diff --git a/README.md b/README.md index 244d3c23..5dd7dfed 100644 --- a/README.md +++ b/README.md @@ -123,21 +123,16 @@ Example for **LineFollowerSim** application: 2. Now the keyboard keys a, b and c can be used to control the robot according to the implemented application logic. ## Communicate with the DroidControlShip -For the communication with the DroidControlShip a socket server needs to be enabled, which is disabled by default. +The communication with the DroidControlShip goes via a Webots serial connection, which is disabled by default. -Use the -s flag to enable it with default port 65432. Note, this will disable the standard logging, because the serial communication will automatically be routed over the socket. The SerialMuxProt protocol is used to exchange data in both directions. +Use the -c flag to enable it with default channels (see _webots_robot_serial_rx_channel_ and _webots_robot_serial_tx_channel_ in [platformio.ini](./platformio.ini)). Note, this will disable the standard logging, because the serial communication uses the SerialMuxProt procotol for data interchange. ```bash -$ program.exe -s +$ program.exe -c ``` -The port can be changed via command line parameters, please use -? to get more details. -```bash -$ program.exe -? -``` - -For simplicity a Platformio project task was added, which start the socket server as well. +For simplicity a Platformio project task was added, which enables the Webots serial connection as well. -![Example](./doc/images/pio_webots_launcher_socket.jpg) +![Example](./doc/images/pio_webots_launcher_zumo_com_system.jpg) # The target @@ -157,11 +152,11 @@ Example for the **LineFollowerTarget** application: | Application | Description | Standalone | DroidControlShop Required | Webots World | | - | - | - | - | - | | Calib | Application used for motor speed calibration. | Yes | No | ./webots/worlds/LargeTrack.wbt ./webots/worlds/LineFollowerTrack.wbt | -| ConvoyLeader | A line follower, providing information to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) in a convoy leader role. | No | Yes | ./webots/worlds/PlatoonTrack.wbt | -| ConvoyFollower | Convoy follower, providing information to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) to drive to its target. | No | Yes | ./webots/worlds/PlatoonTrack.wbt | +| ConvoyLeader | A line follower, providing information to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) in a convoy leader role. | No | Yes | ./webots/worlds/zumo_with_com_system/PlatoonTrack.wbt | +| ConvoyFollower | Convoy follower, providing information to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) to drive to its target. | No | Yes | ./webots/worlds/zumo_with_com_system/PlatoonTrack.wbt | | LineFollower | Just a line follower, using a PID controller. | Yes | No | ./webots/worlds/ETrack.wbt ./webots/worlds/LargeTrack.wbt ./webots/worlds/LineFollowerTrack.wbt | -| RemoteControl | The robot is remote controlled by e.g. the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) in a convoy follower role. | No | Yes | Any | -| SensorFusion | The robot provides odometry and inertial data to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip), which calculates the sensor fusion based location information. | No | Yes | ./webots/worlds/SensorFusionTrack.wbt | +| RemoteControl | The robot is remote controlled by e.g. the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip) in a convoy follower role. | No | Yes | ./webots/world/zumo_with_com_system/* | +| SensorFusion | The robot provides odometry and inertial data to the [DroidControlShip](https://github.com/BlueAndi/DroidControlShip), which calculates the sensor fusion based location information. | No | Yes | ./webots/worlds/zumo_with_com_system/SensorFusionTrack.wbt | | Test | Only for testing purposes on native environment. | Yes | No | N/A | # Documentation diff --git a/doc/images/pio_webots_launcher.jpg b/doc/images/pio_webots_launcher.jpg index e111263f..23df4c5f 100644 Binary files a/doc/images/pio_webots_launcher.jpg and b/doc/images/pio_webots_launcher.jpg differ diff --git a/doc/images/pio_webots_launcher_socket.jpg b/doc/images/pio_webots_launcher_socket.jpg deleted file mode 100644 index aae55734..00000000 Binary files a/doc/images/pio_webots_launcher_socket.jpg and /dev/null differ diff --git a/doc/images/pio_webots_launcher_zumo_com_system.jpg b/doc/images/pio_webots_launcher_zumo_com_system.jpg new file mode 100644 index 00000000..5c873d39 Binary files /dev/null and b/doc/images/pio_webots_launcher_zumo_com_system.jpg differ diff --git a/lib/APPSensorFusion/App.cpp b/lib/APPSensorFusion/App.cpp index 49c9d677..9f39468d 100644 --- a/lib/APPSensorFusion/App.cpp +++ b/lib/APPSensorFusion/App.cpp @@ -39,6 +39,7 @@ #include #include #include +#include /****************************************************************************** * Compiler Switches @@ -67,6 +68,7 @@ void App::setup() { Serial.begin(SERIAL_BAUDRATE); + Logging::disable(); /* Initialize HAL */ Board::getInstance().init(); diff --git a/lib/ArduinoNative/Arduino.cpp b/lib/ArduinoNative/Arduino.cpp index 50022cc9..8872f917 100644 --- a/lib/ArduinoNative/Arduino.cpp +++ b/lib/ArduinoNative/Arduino.cpp @@ -39,16 +39,15 @@ #include -#else +#else /* UNIT_TEST */ #include #include #include -#include "SocketServer.h" #include #include -#endif +#endif /* UNIT_TEST */ /****************************************************************************** * Compiler Switches @@ -67,14 +66,15 @@ /** This type defines the possible program arguments. */ typedef struct { - const char* socketServerPort; /**< Socket server port */ - const char* robotName; /**< Robot name */ - bool isSerialOverSocket; /**< Is serial communication over socket? */ - bool verbose; /**< Show verbose information */ + const char* robotName; /**< Robot name */ + bool isZumoComSystemEnabled; /**< Is the ZumoComSystem enabled? */ + const char* serialRxChannel; /**< Serial Rx channel */ + const char* serialTxChannel; /**< Serial Tx channel */ + bool verbose; /**< Show verbose information */ } PrgArguments; -#endif +#endif /* UNIT_TEST */ /****************************************************************************** * Prototypes @@ -88,7 +88,7 @@ extern void loop(); static int handleCommandLineArguments(PrgArguments& prgArguments, int argc, char** argv); static void showPrgArguments(const PrgArguments& prgArgs); -#endif +#endif /* UNIT_TEST */ /****************************************************************************** * Local Variables @@ -113,22 +113,26 @@ static const int MAX_TIME_STEP = 10; */ static SimTime* gSimTime = nullptr; +/** Supported long program arguments. */ +static const struct option LONG_OPTIONS[] = {{"help", no_argument, nullptr, 0}, + {"serialRxCh", required_argument, nullptr, 0}, + {"serialTxCh", required_argument, nullptr, 0}, + {nullptr, no_argument, nullptr, 0}}; /* Marks the end. */ + /** Program argument default value of the robot name. */ static const char* PRG_ARG_ROBOT_NAME_DEFAULT = ""; -/** Default port used for socket communications. */ -static const char* PRG_ARG_SOCKET_SERVER_PORT_DEFAULT = "65432"; +/** Program argument default value of the flag whether the ZumoComSystem is enabled or not. */ +static bool PRG_ARG_IS_ZUMO_COM_SYSTEM_ENABLED_DEFAULT = false; -/** Program argument default value of the verbose flag. */ -static bool PRG_ARG_VERBOSE_DEFAULT = false; +/** Program argument default value of the serial rx channel. */ +static const char PRG_ARG_SERIAL_RX_CH_DEFAULT[] = "1"; -/** Program argument default value of the serial over socket flag. */ -static bool PRG_ARG_IS_SERIAL_OVER_SOCKET_DEFAULT = false; +/** Program argument default value of the serial tx channel. */ +static const char PRG_ARG_SERIAL_TX_CH_DEFAULT[] = "2"; -/** - * Maximum number of socket connections. - */ -static const uint8_t SOCKET_SERVER_MAX_CONNECTIONS = 1U; +/** Program argument default value of the verbose flag. */ +static bool PRG_ARG_VERBOSE_DEFAULT = false; #endif /* UNIT_TEST */ @@ -175,14 +179,15 @@ extern void delay(unsigned long ms) } } -#else +#else /* UNIT_TEST */ extern int main(int argc, char** argv) { - int status = 0; - Keyboard& keyboard = Board::getInstance().getKeyboard(); - PrgArguments prgArguments; - SocketServer socketStream; + int status = 0; + Board& board = Board::getInstance(); + Keyboard& keyboard = board.getKeyboard(); + WebotsSerialDrv* simSerial = board.getSimSerial(); + PrgArguments prgArguments; printf("\n*** Radon Ulzer ***\n"); @@ -200,31 +205,32 @@ extern int main(int argc, char** argv) showPrgArguments(prgArguments); } - /* Enable socket server? */ - if (true == prgArguments.isSerialOverSocket) + /* It might happen that the user enables the ZumoComSystem, but the + * choosen application doesn't support it. + */ + if ((nullptr == simSerial) && (true == prgArguments.isZumoComSystemEnabled)) { - if (false == socketStream.init(prgArguments.socketServerPort, SOCKET_SERVER_MAX_CONNECTIONS)) - { - printf("Error initializing SocketServer.\n"); - status = -1; - } - else - { - if (true == prgArguments.verbose) - { - printf("SocketServer ready on port %s.\n", prgArguments.socketServerPort); - } + printf("Warning: The application doesn't support the ZumoComSystem.\n"); - Serial.setStream(socketStream); - Logging::disable(); - } + prgArguments.isZumoComSystemEnabled = false; } - if (0 == status) + /* Is ZumoComSystem enabled? */ + if ((nullptr != simSerial) && (true == prgArguments.isZumoComSystemEnabled)) { - /* Get simulation time handler. It will be used by millis() and delay(). */ - gSimTime = &Board::getInstance().getSimTime(); + /* Set serial rx/tx channels for communication with the ZumoComSystem. */ + simSerial->setRxChannel(atoi(prgArguments.serialRxChannel)); + simSerial->setTxChannel(atoi(prgArguments.serialTxChannel)); + + /* Use the serial interface for ZumoComSystem communication and not for + * logging. + */ + Serial.setStream(*simSerial); + Logging::disable(); } + + /* Get simulation time handler. It will be used by millis() and delay(). */ + gSimTime = &board.getSimTime(); } if (0 != status) @@ -249,12 +255,12 @@ extern int main(int argc, char** argv) */ /* Enable all simulation devices. Must be done before any other access to the devices. */ - Board::getInstance().enableSimulationDevices(); + board.enableSimulationDevices(); /* Do one single simulation step to force that all sensors will deliver already data. * Otherwise e.g. the position sensor will provide NaN. * This must be done before setup() is called! - * + * * Prerequisite: The sensors must be enabled! */ if (false == gSimTime->step()) @@ -271,7 +277,6 @@ extern int main(int argc, char** argv) { keyboard.getPressedButtons(); loop(); - socketStream.process(); } } } @@ -314,30 +319,48 @@ extern void delay(unsigned long ms) static int handleCommandLineArguments(PrgArguments& prgArguments, int argc, char** argv) { int status = 0; - const char* availableOptions = "p:n:hs"; + const char* availableOptions = "n:cnvh"; const char* programName = argv[0]; - int option = getopt(argc, argv, availableOptions); + int optionIndex = 0; + int option = getopt_long(argc, argv, availableOptions, LONG_OPTIONS, &optionIndex); /* Set default values */ - prgArguments.socketServerPort = PRG_ARG_SOCKET_SERVER_PORT_DEFAULT; - prgArguments.robotName = PRG_ARG_ROBOT_NAME_DEFAULT; - prgArguments.verbose = PRG_ARG_VERBOSE_DEFAULT; - prgArguments.isSerialOverSocket = PRG_ARG_IS_SERIAL_OVER_SOCKET_DEFAULT; + prgArguments.robotName = PRG_ARG_ROBOT_NAME_DEFAULT; + prgArguments.isZumoComSystemEnabled = PRG_ARG_IS_ZUMO_COM_SYSTEM_ENABLED_DEFAULT; + prgArguments.serialRxChannel = PRG_ARG_SERIAL_RX_CH_DEFAULT; + prgArguments.serialTxChannel = PRG_ARG_SERIAL_TX_CH_DEFAULT; + prgArguments.verbose = PRG_ARG_VERBOSE_DEFAULT; while ((-1 != option) && (0 == status)) { switch (option) { - case 'n': /* Name */ - prgArguments.robotName = optarg; + case 0: /* Long option */ + + if (0 == strcmp(LONG_OPTIONS[optionIndex].name, "help")) + { + status = -1; + } + else if (0 == strcmp(LONG_OPTIONS[optionIndex].name, "serialRxCh")) + { + prgArguments.serialRxChannel = optarg; + } + else if (0 == strcmp(LONG_OPTIONS[optionIndex].name, "serialTxCh")) + { + prgArguments.serialTxChannel = optarg; + } + else + { + status = -1; + } break; - case 'p': /* SocketServer Port */ - prgArguments.socketServerPort = optarg; + case 'c': /* Is ZumoComSystem enabled? */ + prgArguments.isZumoComSystemEnabled = true; break; - case 's': /* Is serial over socket? */ - prgArguments.isSerialOverSocket = true; + case 'n': /* Name */ + prgArguments.robotName = optarg; break; case 'v': /* Verbose */ @@ -355,19 +378,21 @@ static int handleCommandLineArguments(PrgArguments& prgArguments, int argc, char break; } - option = getopt(argc, argv, availableOptions); + option = getopt_long(argc, argv, availableOptions, LONG_OPTIONS, &optionIndex); } /* Does the user need help? */ if (0 > status) { printf("Usage: %s \nOptions:\n", programName); - printf("\t-h\t\t\tShow this help message.\n"); /* Help */ - printf("\t-n \t\tSet robot name.\n"); /* Robot Name */ - printf("\t-p \tSet SocketServer port."); /* SocketServer Port */ - printf(" Default: %s\n", PRG_ARG_SOCKET_SERVER_PORT_DEFAULT); /* SocketServer port default value */ - printf("\t-s\t\t\tEnable serial over socket.\n"); /* Flag */ - printf("\t-v\t\t\tVerbose mode.\n"); /* Flag */ + printf("\t-h\t\t\tShow this help message.\n"); /* Help */ + printf("\t-n \t\tSet robot name.\n"); /* Robot Name */ + printf("\t-c\t\t\tEnable ZumoComSystem. Default: Disabled\n"); /* Flag */ + printf("\t--serialRxCh \t\tSet serial rx channel (ZumoComSystem)."); /* Serial rx channel */ + printf(" Default: %s\n", PRG_ARG_SERIAL_RX_CH_DEFAULT); /* Serial rx channel default value */ + printf("\t--serialTxCh \t\tSet serial tx channel (ZumoComSystem)."); /* Serial txchannel */ + printf(" Default: %s\n", PRG_ARG_SERIAL_TX_CH_DEFAULT); /* Serial tx channel default value */ + printf("\t-v\t\t\tVerbose mode. Default: Disabled\n"); /* Flag */ } return status; @@ -380,10 +405,11 @@ static int handleCommandLineArguments(PrgArguments& prgArguments, int argc, char */ static void showPrgArguments(const PrgArguments& prgArgs) { - printf("Robot name : %s\n", prgArgs.robotName); - printf("SocketServer Port : %s\n", prgArgs.socketServerPort); - printf("Serial over socket: %s\n", (false == prgArgs.isSerialOverSocket) ? "disabled" : "enabled"); + printf("Robot name : %s\n", prgArgs.robotName); + printf("ZumoComSystem : %s\n", (false == prgArgs.isZumoComSystemEnabled) ? "disabled" : "enabled"); + printf("Serial rx channel: %s\n", prgArgs.serialRxChannel); + printf("Serial tx channel: %s\n", prgArgs.serialTxChannel); /* Skip verbose flag. */ } -#endif \ No newline at end of file +#endif /* UNIT_TEST */ \ No newline at end of file diff --git a/lib/ArduinoNative/SocketServer.cpp b/lib/ArduinoNative/SocketServer.cpp deleted file mode 100644 index 4ce3c61c..00000000 --- a/lib/ArduinoNative/SocketServer.cpp +++ /dev/null @@ -1,444 +0,0 @@ -/* MIT License - * - * Copyright (c) 2023 - 2024 Andreas Merkle - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -/******************************************************************************* - DESCRIPTION -*******************************************************************************/ -/** - * @brief Socket Server for Inter-Process Communication - * @author Gabryel Reyes - */ - -/****************************************************************************** - * Includes - *****************************************************************************/ - -#include "SocketServer.h" -#include -#include -#include - -#ifdef _WIN32 -#undef UNICODE -#define WIN32_LEAN_AND_MEAN -#include -#include -#include -#pragma comment(lib, "Ws2_32.lib") -#else -#include /* definition of inet_ntoa */ -#include /* definition of gethostbyname */ -#include /* definition of struct sockaddr_in */ -#include -#include -#include /* definition of close */ -#include /* definition of memset for tests. */ -#endif - -/****************************************************************************** - * Macros - *****************************************************************************/ - -#ifndef _WIN32 -#define INVALID_SOCKET (SOCKET)(~0) -#define SOCKET_ERROR (-1) -#endif - -/****************************************************************************** - * Types and classes - *****************************************************************************/ - -#ifndef _WIN32 -typedef unsigned int UINT_PTR; -typedef UINT_PTR SOCKET; -#endif - -/****************************************************************************** - * Prototypes - *****************************************************************************/ - -/****************************************************************************** - * Local Variables - *****************************************************************************/ - -/** SocketServer Members. PIMPL Idiom. */ -struct SocketServer::SocketServerImpl -{ - /** - * File Descriptor of the Client Socket. - */ - SOCKET m_clientSocket; - - /** - * File Descriptor of the Listening Socket. - */ - SOCKET m_listenSocket; - - /** - * Queue for the received bytes. - */ - std::queue m_rcvQueue; - - /** - * Construct an SocketServerImpl instance. - */ - SocketServerImpl() : m_clientSocket(INVALID_SOCKET), m_listenSocket(INVALID_SOCKET), m_rcvQueue() - { - } -}; - -/****************************************************************************** - * Public Methods - *****************************************************************************/ - -SocketServer::SocketServer() : Stream(), m_members(new SocketServerImpl) -{ -} - -SocketServer::~SocketServer() -{ - /* Sockets are closed before deleting m_members. */ - closeListeningSocket(); - - if (nullptr != m_members) - { - delete m_members; - } -} - -bool SocketServer::init(const char* port, uint8_t maxConnections) -{ - int result; - struct addrinfo hints; - struct addrinfo* addrInfo = nullptr; - - if (nullptr == m_members) - { - return false; - } - - memset(&hints, 0, sizeof(struct addrinfo)); - - hints.ai_family = AF_INET; - hints.ai_socktype = SOCK_STREAM; - hints.ai_protocol = IPPROTO_TCP; - hints.ai_flags = AI_PASSIVE; - -#ifdef _WIN32 - WSADATA wsaData; - - /* Initialize Winsock */ - result = WSAStartup(MAKEWORD(2, 2), &wsaData); - if (0 != result) - { - printf("WSAStartup failed with error: %d\n", result); - return false; - } - -#endif - - /* Resolve the server address and port */ - result = getaddrinfo(nullptr, port, &hints, &addrInfo); - if (0 != result) - { - printf("getaddrinfo failed with error: %d\n", result); - closeListeningSocket(); - return false; - } - - /* Create a SOCKET for the server to listen for client connections. */ - m_members->m_listenSocket = socket(addrInfo->ai_family, addrInfo->ai_socktype, addrInfo->ai_protocol); - if (INVALID_SOCKET == m_members->m_listenSocket) - { - printf("socket failed\n"); - freeaddrinfo(addrInfo); - closeListeningSocket(); - return false; - } - - /* Setup the TCP listening socket */ - result = bind(m_members->m_listenSocket, addrInfo->ai_addr, static_cast(addrInfo->ai_addrlen)); - if (SOCKET_ERROR == result) - { - printf("bind failed\n"); - freeaddrinfo(addrInfo); - closeListeningSocket(); - return false; - } - - freeaddrinfo(addrInfo); - - result = listen(m_members->m_listenSocket, maxConnections); - if (SOCKET_ERROR == result) - { - printf("listen failed\n"); - closeListeningSocket(); - return false; - } - - return true; -} - -void SocketServer::print(const char str[]) -{ - /* Not implemented*/ - (void)str; -} - -void SocketServer::print(uint8_t value) -{ - /* Not implemented*/ - (void)value; -} - -void SocketServer::print(uint16_t value) -{ - /* Not implemented*/ - (void)value; -} - -void SocketServer::print(uint32_t value) -{ - /* Not implemented*/ - (void)value; -} - -void SocketServer::print(int8_t value) -{ - /* Not implemented*/ - (void)value; -} - -void SocketServer::print(int16_t value) -{ - /* Not implemented*/ - (void)value; -} - -void SocketServer::print(int32_t value) -{ - /* Not implemented*/ - (void)value; -} - -void SocketServer::println(const char str[]) -{ - /* Not implemented*/ - (void)str; -} - -void SocketServer::println(uint8_t value) -{ - /* Not implemented*/ - (void)value; -} - -void SocketServer::println(uint16_t value) -{ - /* Not implemented*/ - (void)value; -} - -void SocketServer::println(uint32_t value) -{ - /* Not implemented*/ - (void)value; -} - -void SocketServer::println(int8_t value) -{ - /* Not implemented*/ - (void)value; -} - -void SocketServer::println(int16_t value) -{ - /* Not implemented*/ - (void)value; -} - -void SocketServer::println(int32_t value) -{ - /* Not implemented*/ - (void)value; -} - -size_t SocketServer::write(const uint8_t* buffer, size_t length) -{ - size_t bytesSent = 0; - - if (nullptr != m_members) - { - /* Echo the buffer back to the sender */ - if (INVALID_SOCKET != m_members->m_clientSocket) - { - int result = send(m_members->m_clientSocket, reinterpret_cast(buffer), length, 0); - if (SOCKET_ERROR == result) - { - printf("send failed\n"); - /* Error on the socket. Client is now invalid. */ - m_members->m_clientSocket = INVALID_SOCKET; - } - else - { - bytesSent = result; - } - } - } - return bytesSent; -} - -int SocketServer::available() const -{ - return (nullptr != m_members) ? m_members->m_rcvQueue.size() : 0; -} - -size_t SocketServer::readBytes(uint8_t* buffer, size_t length) -{ - size_t count = 0; - - for (count = 0; count < length; count++) - { - uint8_t byte; - if (false == getByte(byte)) - { - break; - } - else - { - buffer[count] = byte; - } - } - - return count; -} - -void SocketServer::process() -{ - if (nullptr != m_members) - { - if (INVALID_SOCKET != m_members->m_listenSocket) - { - fd_set readFDS, writeFDS, exceptFDS; - int result; - struct timeval timeout; - - timeout.tv_sec = 0; - timeout.tv_usec = 0; - - FD_ZERO(&readFDS); - FD_ZERO(&writeFDS); - FD_ZERO(&exceptFDS); - - FD_SET(m_members->m_listenSocket, &readFDS); - FD_SET(m_members->m_listenSocket, &exceptFDS); - - /* If there is a client connected */ - if (INVALID_SOCKET != m_members->m_clientSocket) - { - FD_SET(m_members->m_clientSocket, &readFDS); - FD_SET(m_members->m_clientSocket, &exceptFDS); - } - - result = select(m_members->m_listenSocket + 1, &readFDS, &writeFDS, &exceptFDS, &timeout); - - if (0 < result) - { - /* New Client Connection available */ - if (FD_ISSET(m_members->m_listenSocket, &readFDS)) - { - /* Accept a client socket */ - m_members->m_clientSocket = accept(m_members->m_listenSocket, nullptr, nullptr); - if (INVALID_SOCKET == m_members->m_clientSocket) - { - printf("accept failed\n"); - } - } - - /* Client Ready to read */ - if (FD_ISSET(m_members->m_clientSocket, &readFDS)) - { - const size_t bufferLength = UINT8_MAX; - char recvbuf[bufferLength]; - int result = recv(m_members->m_clientSocket, recvbuf, bufferLength, 0); - - if (0 < result) - { - for (uint8_t idx = 0; idx < result; idx++) - { - m_members->m_rcvQueue.push(static_cast(recvbuf[idx])); - } - } - else - { - /* Client disconnected or error on the socket. */ - m_members->m_clientSocket = INVALID_SOCKET; - } - } - } - } - } -} - -/****************************************************************************** - * Private Methods - *****************************************************************************/ - -void SocketServer::closeListeningSocket() -{ - if (nullptr != m_members) - { - /* Close the listening socket. */ - if (INVALID_SOCKET != m_members->m_listenSocket) - { -#ifdef _WIN32 - closesocket(m_members->m_listenSocket); -#else - close(m_members->m_listenSocket); -#endif - } - } - -#ifdef _WIN32 - /* Terminate the use of the Winsock 2 DLL. */ - WSACleanup(); -#endif -} - -bool SocketServer::getByte(uint8_t& byte) -{ - if (nullptr != m_members) - { - if (false == m_members->m_rcvQueue.empty()) - { - byte = m_members->m_rcvQueue.front(); - m_members->m_rcvQueue.pop(); - return true; - } - } - return false; -} - -/****************************************************************************** - * Local Functions - *****************************************************************************/ diff --git a/lib/HALCalibSim/Board.cpp b/lib/HALCalibSim/Board.cpp index 86c04574..21c95b4b 100644 --- a/lib/HALCalibSim/Board.cpp +++ b/lib/HALCalibSim/Board.cpp @@ -33,6 +33,7 @@ * Includes *****************************************************************************/ #include +#include /****************************************************************************** * Compiler Switches @@ -54,72 +55,6 @@ * Local Variables *****************************************************************************/ -/* Name of the speaker in the robot simulation. */ -const char* Board::SPEAKER_NAME = "speaker"; - -/* Name of the display in the robot simulation. */ -const char* Board::DISPLAY_NAME = "robot_display"; - -/* Name of the left motor in the robot simulation. */ -const char* Board::LEFT_MOTOR_NAME = "motor_left"; - -/* Name of the right motor in the robot simulation. */ -const char* Board::RIGHT_MOTOR_NAME = "motor_right"; - -/* Name of the infrared emitter 0 in the robot simulation. */ -const char* Board::EMITTER_0_NAME = "emitter_l"; - -/* Name of the infrared emitter 1 in the robot simulation. */ -const char* Board::EMITTER_1_NAME = "emitter_lm"; - -/* Name of the infrared emitter 2 in the robot simulation. */ -const char* Board::EMITTER_2_NAME = "emitter_m"; - -/* Name of the infrared emitter 3 in the robot simulation. */ -const char* Board::EMITTER_3_NAME = "emitter_rm"; - -/* Name of the infrared emitter 4 in the robot simulation. */ -const char* Board::EMITTER_4_NAME = "emitter_r"; - -/** Name of the position sensor of the left motor in the robot simulation. */ -const char* Board::POS_SENSOR_LEFT_NAME = "position_sensor_left"; - -/** Name of the position sensor of the right motor in the robot simulation. */ -const char* Board::POS_SENSOR_RIGHT_NAME = "position_sensor_right"; - -/* Name of the light sensor 0 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_0_NAME = "lightsensor_l"; - -/* Name of the light sensor 1 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_1_NAME = "lightsensor_lm"; - -/* Name of the light sensor 2 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_2_NAME = "lightsensor_m"; - -/* Name of the light sensor 3 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_3_NAME = "lightsensor_rm"; - -/* Name of the light sensor 4 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_4_NAME = "lightsensor_r"; - -/* Name of the red LED in the robot simulation. */ -const char* Board::LED_RED_NAME = "led_red"; - -/* Name of the yellow LED in the robot simulation. */ -const char* Board::LED_YELLOW_NAME = "led_yellow"; - -/* Name of the green LED in the robot simulation. */ -const char* Board::LED_GREEN_NAME = "led_green"; - -/* Name of the front left proximity sensor in the robot simulation. */ -const char* Board::PROXIMITY_SENSOR_FRONT_LEFT_NAME = "proxim_sensor_fl"; - -/* Name of the front right proximity sensor in the robot simulation. */ -const char* Board::PROXIMITY_SENSOR_FRONT_RIGHT_NAME = "proxim_sensor_fr"; - -/* Name of the sender in the robot simulation. */ -const char* Board::SENDER_NAME = "serialComTx"; - /****************************************************************************** * Public Methods *****************************************************************************/ @@ -149,21 +84,26 @@ Board::Board() : m_buttonA(m_keyboard), m_buttonB(m_keyboard), m_buttonC(m_keyboard), - m_buzzer(m_robot.getSpeaker(SPEAKER_NAME)), - m_display(m_robot.getDisplay(DISPLAY_NAME)), - m_encoders(m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME), m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)), - m_lineSensors(m_robot.getEmitter(EMITTER_0_NAME), m_robot.getEmitter(EMITTER_1_NAME), - m_robot.getEmitter(EMITTER_2_NAME), m_robot.getEmitter(EMITTER_3_NAME), - m_robot.getEmitter(EMITTER_4_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)), - m_motors(m_robot.getMotor(LEFT_MOTOR_NAME), m_robot.getMotor(RIGHT_MOTOR_NAME)), - m_ledRed(m_robot.getLED(LED_RED_NAME)), - m_ledYellow(m_robot.getLED(LED_YELLOW_NAME)), - m_ledGreen(m_robot.getLED(LED_GREEN_NAME)), - m_proximitySensors(m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_LEFT_NAME), - m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_RIGHT_NAME)), - m_sender(m_robot.getEmitter(SENDER_NAME)), + m_buzzer(m_robot.getSpeaker(RobotDeviceNames::SPEAKER_NAME)), + m_display(m_robot.getDisplay(RobotDeviceNames::DISPLAY_NAME)), + m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), + m_lineSensors( + m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), + m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), + m_ledRed(m_robot.getLED(RobotDeviceNames::LED_RED_NAME)), + m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), + m_ledGreen(m_robot.getLED(RobotDeviceNames::LED_GREEN_NAME)), + m_proximitySensors(m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)), + m_sender(m_robot.getEmitter(RobotDeviceNames::EMITTER_NAME_SERIAL)), m_settings() { } @@ -173,15 +113,15 @@ void Board::enableSimulationDevices() const int timeStep = m_simTime.getTimeStep(); m_robot.getKeyboard()->enable(timeStep); - m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME)->enable(timeStep); - m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)->enable(timeStep); - m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_LEFT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_RIGHT_NAME)->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)->enable(timeStep); } /****************************************************************************** diff --git a/lib/HALCalibSim/Board.h b/lib/HALCalibSim/Board.h index fa2d0826..7b0100d6 100644 --- a/lib/HALCalibSim/Board.h +++ b/lib/HALCalibSim/Board.h @@ -63,6 +63,7 @@ #include #include #include +#include /****************************************************************************** * Macros @@ -243,73 +244,7 @@ class Board : public IBoard m_buzzer.process(); } -protected: private: - /** Name of the speaker in the robot simulation. */ - static const char* SPEAKER_NAME; - - /** Name of the display in the robot simulation. */ - static const char* DISPLAY_NAME; - - /** Name of the left motor in the robot simulation. */ - static const char* LEFT_MOTOR_NAME; - - /** Name of the right motor in the robot simulation. */ - static const char* RIGHT_MOTOR_NAME; - - /** Name of the infrared emitter 0 in the robot simulation. */ - static const char* EMITTER_0_NAME; - - /** Name of the infrared emitter 1 in the robot simulation. */ - static const char* EMITTER_1_NAME; - - /** Name of the infrared emitter 2 in the robot simulation. */ - static const char* EMITTER_2_NAME; - - /** Name of the infrared emitter 3 in the robot simulation. */ - static const char* EMITTER_3_NAME; - - /** Name of the infrared emitter 4 in the robot simulation. */ - static const char* EMITTER_4_NAME; - - /** Name of the position sensor of the left motor in the robot simulation. */ - static const char* POS_SENSOR_LEFT_NAME; - - /** Name of the position sensor of the right motor in the robot simulation. */ - static const char* POS_SENSOR_RIGHT_NAME; - - /** Name of the light sensor 0 in the robot simulation. */ - static const char* LIGHT_SENSOR_0_NAME; - - /** Name of the light sensor 1 in the robot simulation. */ - static const char* LIGHT_SENSOR_1_NAME; - - /** Name of the light sensor 2 in the robot simulation. */ - static const char* LIGHT_SENSOR_2_NAME; - - /** Name of the light sensor 3 in the robot simulation. */ - static const char* LIGHT_SENSOR_3_NAME; - - /** Name of the light sensor 4 in the robot simulation. */ - static const char* LIGHT_SENSOR_4_NAME; - - /** Name of the red LED in the robot simulation. */ - static const char* LED_RED_NAME; - - /** Name of the yellow LED in the robot simulation. */ - static const char* LED_YELLOW_NAME; - - /** Name of the green LED in the robot simulation. */ - static const char* LED_GREEN_NAME; - - /** Name of the front proximity sensor in the robot simulation. */ - static const char* PROXIMITY_SENSOR_FRONT_LEFT_NAME; - - /** Name of the front right proximity sensor in the robot simulation. */ - static const char* PROXIMITY_SENSOR_FRONT_RIGHT_NAME; - - /** Name of the sender in the robot simulation. */ - static const char* SENDER_NAME; /** Simulated roboter instance. */ webots::Robot m_robot; @@ -394,6 +329,17 @@ class Board : public IBoard return m_keyboard; } + /** + * Get the simulation serial driver, which is connected within Webots. + * + * @return If serial driver is available, it will return a pointer to it, otherwise nullptr. + */ + WebotsSerialDrv* getSimSerial() + { + /* Not supported. */ + return nullptr; + } + /** * Enable all simulation devices. * It is called by the main entry only. diff --git a/lib/HALConvoyFollowerSim/Board.cpp b/lib/HALConvoyFollowerSim/Board.cpp index 21a8f080..11ce3e23 100644 --- a/lib/HALConvoyFollowerSim/Board.cpp +++ b/lib/HALConvoyFollowerSim/Board.cpp @@ -33,6 +33,7 @@ * Includes *****************************************************************************/ #include +#include /****************************************************************************** * Compiler Switches @@ -54,69 +55,6 @@ * Local Variables *****************************************************************************/ -/* Name of the speaker in the robot simulation. */ -const char* Board::SPEAKER_NAME = "speaker"; - -/* Name of the display in the robot simulation. */ -const char* Board::DISPLAY_NAME = "robot_display"; - -/* Name of the left motor in the robot simulation. */ -const char* Board::LEFT_MOTOR_NAME = "motor_left"; - -/* Name of the right motor in the robot simulation. */ -const char* Board::RIGHT_MOTOR_NAME = "motor_right"; - -/* Name of the infrared emitter 0 in the robot simulation. */ -const char* Board::EMITTER_0_NAME = "emitter_l"; - -/* Name of the infrared emitter 1 in the robot simulation. */ -const char* Board::EMITTER_1_NAME = "emitter_lm"; - -/* Name of the infrared emitter 2 in the robot simulation. */ -const char* Board::EMITTER_2_NAME = "emitter_m"; - -/* Name of the infrared emitter 3 in the robot simulation. */ -const char* Board::EMITTER_3_NAME = "emitter_rm"; - -/* Name of the infrared emitter 4 in the robot simulation. */ -const char* Board::EMITTER_4_NAME = "emitter_r"; - -/** Name of the position sensor of the left motor in the robot simulation. */ -const char* Board::POS_SENSOR_LEFT_NAME = "position_sensor_left"; - -/** Name of the position sensor of the right motor in the robot simulation. */ -const char* Board::POS_SENSOR_RIGHT_NAME = "position_sensor_right"; - -/* Name of the light sensor 0 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_0_NAME = "lightsensor_l"; - -/* Name of the light sensor 1 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_1_NAME = "lightsensor_lm"; - -/* Name of the light sensor 2 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_2_NAME = "lightsensor_m"; - -/* Name of the light sensor 3 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_3_NAME = "lightsensor_rm"; - -/* Name of the light sensor 4 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_4_NAME = "lightsensor_r"; - -/* Name of the red LED in the robot simulation. */ -const char* Board::LED_RED_NAME = "led_red"; - -/* Name of the yellow LED in the robot simulation. */ -const char* Board::LED_YELLOW_NAME = "led_yellow"; - -/* Name of the green LED in the robot simulation. */ -const char* Board::LED_GREEN_NAME = "led_green"; - -/* Name of the front left proximity sensor in the robot simulation. */ -const char* Board::PROXIMITY_SENSOR_FRONT_LEFT_NAME = "proxim_sensor_fl"; - -/* Name of the front right proximity sensor in the robot simulation. */ -const char* Board::PROXIMITY_SENSOR_FRONT_RIGHT_NAME = "proxim_sensor_fr"; - /****************************************************************************** * Public Methods *****************************************************************************/ @@ -146,20 +84,27 @@ Board::Board() : m_buttonA(m_keyboard), m_buttonB(m_keyboard), m_buttonC(m_keyboard), - m_buzzer(m_robot.getSpeaker(SPEAKER_NAME)), - m_display(m_robot.getDisplay(DISPLAY_NAME)), - m_encoders(m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME), m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)), - m_lineSensors(m_robot.getEmitter(EMITTER_0_NAME), m_robot.getEmitter(EMITTER_1_NAME), - m_robot.getEmitter(EMITTER_2_NAME), m_robot.getEmitter(EMITTER_3_NAME), - m_robot.getEmitter(EMITTER_4_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)), - m_motors(m_robot.getMotor(LEFT_MOTOR_NAME), m_robot.getMotor(RIGHT_MOTOR_NAME)), - m_ledRed(m_robot.getLED(LED_RED_NAME)), - m_ledYellow(m_robot.getLED(LED_YELLOW_NAME)), - m_ledGreen(m_robot.getLED(LED_GREEN_NAME)), - m_proximitySensors(m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_LEFT_NAME), - m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_RIGHT_NAME)), + m_buzzer(m_robot.getSpeaker(RobotDeviceNames::SPEAKER_NAME)), + m_display(m_robot.getDisplay(RobotDeviceNames::DISPLAY_NAME)), + m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), + m_lineSensors( + m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), + m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), + m_ledRed(m_robot.getLED(RobotDeviceNames::LED_RED_NAME)), + m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), + m_ledGreen(m_robot.getLED(RobotDeviceNames::LED_GREEN_NAME)), + m_proximitySensors(m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)), + m_serialDrv(m_robot.getEmitter(RobotDeviceNames::EMITTER_NAME_SERIAL), + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)), m_settings() { } @@ -169,15 +114,16 @@ void Board::enableSimulationDevices() const int timeStep = m_simTime.getTimeStep(); m_robot.getKeyboard()->enable(timeStep); - m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME)->enable(timeStep); - m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)->enable(timeStep); - m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_LEFT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_RIGHT_NAME)->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)->enable(timeStep); + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)->enable(timeStep); } /****************************************************************************** * External Functions diff --git a/lib/HALConvoyFollowerSim/Board.h b/lib/HALConvoyFollowerSim/Board.h index 50b27ee4..1ee66215 100644 --- a/lib/HALConvoyFollowerSim/Board.h +++ b/lib/HALConvoyFollowerSim/Board.h @@ -62,6 +62,7 @@ #include #include #include +#include /****************************************************************************** * Macros @@ -233,68 +234,6 @@ class Board : public IBoard } private: - /** Name of the speaker in the robot simulation. */ - static const char* SPEAKER_NAME; - - /** Name of the display in the robot simulation. */ - static const char* DISPLAY_NAME; - - /** Name of the left motor in the robot simulation. */ - static const char* LEFT_MOTOR_NAME; - - /** Name of the right motor in the robot simulation. */ - static const char* RIGHT_MOTOR_NAME; - - /** Name of the infrared emitter 0 in the robot simulation. */ - static const char* EMITTER_0_NAME; - - /** Name of the infrared emitter 1 in the robot simulation. */ - static const char* EMITTER_1_NAME; - - /** Name of the infrared emitter 2 in the robot simulation. */ - static const char* EMITTER_2_NAME; - - /** Name of the infrared emitter 3 in the robot simulation. */ - static const char* EMITTER_3_NAME; - - /** Name of the infrared emitter 4 in the robot simulation. */ - static const char* EMITTER_4_NAME; - - /** Name of the position sensor of the left motor in the robot simulation. */ - static const char* POS_SENSOR_LEFT_NAME; - - /** Name of the position sensor of the right motor in the robot simulation. */ - static const char* POS_SENSOR_RIGHT_NAME; - - /** Name of the light sensor 0 in the robot simulation. */ - static const char* LIGHT_SENSOR_0_NAME; - - /** Name of the light sensor 1 in the robot simulation. */ - static const char* LIGHT_SENSOR_1_NAME; - - /** Name of the light sensor 2 in the robot simulation. */ - static const char* LIGHT_SENSOR_2_NAME; - - /** Name of the light sensor 3 in the robot simulation. */ - static const char* LIGHT_SENSOR_3_NAME; - - /** Name of the light sensor 4 in the robot simulation. */ - static const char* LIGHT_SENSOR_4_NAME; - - /** Name of the red LED in the robot simulation. */ - static const char* LED_RED_NAME; - - /** Name of the yellow LED in the robot simulation. */ - static const char* LED_YELLOW_NAME; - - /** Name of the green LED in the robot simulation. */ - static const char* LED_GREEN_NAME; - - /** Name of the front proximity sensor in the robot simulation. */ - static const char* PROXIMITY_SENSOR_FRONT_LEFT_NAME; - - /** Name of the front right proximity sensor in the robot simulation. */ - static const char* PROXIMITY_SENSOR_FRONT_RIGHT_NAME; /** Simulated roboter instance. */ webots::Robot m_robot; @@ -341,6 +280,9 @@ class Board : public IBoard /** Proximity sensors */ ProximitySensors m_proximitySensors; + /** Simulation serial driver */ + WebotsSerialDrv m_serialDrv; + /** Settings */ Settings m_settings; @@ -376,6 +318,16 @@ class Board : public IBoard return m_keyboard; } + /** + * Get the simulation serial driver, which is connected within Webots. + * + * @return If serial driver is available, it will return a pointer to it, otherwise nullptr. + */ + WebotsSerialDrv* getSimSerial() + { + return &m_serialDrv; + } + /** * Enable all simulation devices. * It is called by the main entry only. diff --git a/lib/HALConvoyLeaderSim/Board.cpp b/lib/HALConvoyLeaderSim/Board.cpp index 4ce7f198..b6721de9 100644 --- a/lib/HALConvoyLeaderSim/Board.cpp +++ b/lib/HALConvoyLeaderSim/Board.cpp @@ -33,6 +33,7 @@ * Includes *****************************************************************************/ #include +#include /****************************************************************************** * Compiler Switches @@ -54,69 +55,6 @@ * Local Variables *****************************************************************************/ -/* Name of the speaker in the robot simulation. */ -const char* Board::SPEAKER_NAME = "speaker"; - -/* Name of the display in the robot simulation. */ -const char* Board::DISPLAY_NAME = "robot_display"; - -/* Name of the left motor in the robot simulation. */ -const char* Board::LEFT_MOTOR_NAME = "motor_left"; - -/* Name of the right motor in the robot simulation. */ -const char* Board::RIGHT_MOTOR_NAME = "motor_right"; - -/* Name of the infrared emitter 0 in the robot simulation. */ -const char* Board::EMITTER_0_NAME = "emitter_l"; - -/* Name of the infrared emitter 1 in the robot simulation. */ -const char* Board::EMITTER_1_NAME = "emitter_lm"; - -/* Name of the infrared emitter 2 in the robot simulation. */ -const char* Board::EMITTER_2_NAME = "emitter_m"; - -/* Name of the infrared emitter 3 in the robot simulation. */ -const char* Board::EMITTER_3_NAME = "emitter_rm"; - -/* Name of the infrared emitter 4 in the robot simulation. */ -const char* Board::EMITTER_4_NAME = "emitter_r"; - -/** Name of the position sensor of the left motor in the robot simulation. */ -const char* Board::POS_SENSOR_LEFT_NAME = "position_sensor_left"; - -/** Name of the position sensor of the right motor in the robot simulation. */ -const char* Board::POS_SENSOR_RIGHT_NAME = "position_sensor_right"; - -/* Name of the light sensor 0 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_0_NAME = "lightsensor_l"; - -/* Name of the light sensor 1 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_1_NAME = "lightsensor_lm"; - -/* Name of the light sensor 2 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_2_NAME = "lightsensor_m"; - -/* Name of the light sensor 3 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_3_NAME = "lightsensor_rm"; - -/* Name of the light sensor 4 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_4_NAME = "lightsensor_r"; - -/* Name of the red LED in the robot simulation. */ -const char* Board::LED_RED_NAME = "led_red"; - -/* Name of the yellow LED in the robot simulation. */ -const char* Board::LED_YELLOW_NAME = "led_yellow"; - -/* Name of the green LED in the robot simulation. */ -const char* Board::LED_GREEN_NAME = "led_green"; - -/* Name of the front left proximity sensor in the robot simulation. */ -const char* Board::PROXIMITY_SENSOR_FRONT_LEFT_NAME = "proxim_sensor_fl"; - -/* Name of the front right proximity sensor in the robot simulation. */ -const char* Board::PROXIMITY_SENSOR_FRONT_RIGHT_NAME = "proxim_sensor_fr"; - /****************************************************************************** * Public Methods *****************************************************************************/ @@ -146,20 +84,27 @@ Board::Board() : m_buttonA(m_keyboard), m_buttonB(m_keyboard), m_buttonC(m_keyboard), - m_buzzer(m_robot.getSpeaker(SPEAKER_NAME)), - m_display(m_robot.getDisplay(DISPLAY_NAME)), - m_encoders(m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME), m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)), - m_lineSensors(m_robot.getEmitter(EMITTER_0_NAME), m_robot.getEmitter(EMITTER_1_NAME), - m_robot.getEmitter(EMITTER_2_NAME), m_robot.getEmitter(EMITTER_3_NAME), - m_robot.getEmitter(EMITTER_4_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)), - m_motors(m_robot.getMotor(LEFT_MOTOR_NAME), m_robot.getMotor(RIGHT_MOTOR_NAME)), - m_ledRed(m_robot.getLED(LED_RED_NAME)), - m_ledYellow(m_robot.getLED(LED_YELLOW_NAME)), - m_ledGreen(m_robot.getLED(LED_GREEN_NAME)), - m_proximitySensors(m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_LEFT_NAME), - m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_RIGHT_NAME)), + m_buzzer(m_robot.getSpeaker(RobotDeviceNames::SPEAKER_NAME)), + m_display(m_robot.getDisplay(RobotDeviceNames::DISPLAY_NAME)), + m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), + m_lineSensors( + m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), + m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), + m_ledRed(m_robot.getLED(RobotDeviceNames::LED_RED_NAME)), + m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), + m_ledGreen(m_robot.getLED(RobotDeviceNames::LED_GREEN_NAME)), + m_proximitySensors(m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)), + m_serialDrv(m_robot.getEmitter(RobotDeviceNames::EMITTER_NAME_SERIAL), + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)), m_settings() { } @@ -169,15 +114,16 @@ void Board::enableSimulationDevices() const int timeStep = m_simTime.getTimeStep(); m_robot.getKeyboard()->enable(timeStep); - m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME)->enable(timeStep); - m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)->enable(timeStep); - m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_LEFT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_RIGHT_NAME)->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)->enable(timeStep); + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)->enable(timeStep); } /****************************************************************************** diff --git a/lib/HALConvoyLeaderSim/Board.h b/lib/HALConvoyLeaderSim/Board.h index 50b27ee4..1ee66215 100644 --- a/lib/HALConvoyLeaderSim/Board.h +++ b/lib/HALConvoyLeaderSim/Board.h @@ -62,6 +62,7 @@ #include #include #include +#include /****************************************************************************** * Macros @@ -233,68 +234,6 @@ class Board : public IBoard } private: - /** Name of the speaker in the robot simulation. */ - static const char* SPEAKER_NAME; - - /** Name of the display in the robot simulation. */ - static const char* DISPLAY_NAME; - - /** Name of the left motor in the robot simulation. */ - static const char* LEFT_MOTOR_NAME; - - /** Name of the right motor in the robot simulation. */ - static const char* RIGHT_MOTOR_NAME; - - /** Name of the infrared emitter 0 in the robot simulation. */ - static const char* EMITTER_0_NAME; - - /** Name of the infrared emitter 1 in the robot simulation. */ - static const char* EMITTER_1_NAME; - - /** Name of the infrared emitter 2 in the robot simulation. */ - static const char* EMITTER_2_NAME; - - /** Name of the infrared emitter 3 in the robot simulation. */ - static const char* EMITTER_3_NAME; - - /** Name of the infrared emitter 4 in the robot simulation. */ - static const char* EMITTER_4_NAME; - - /** Name of the position sensor of the left motor in the robot simulation. */ - static const char* POS_SENSOR_LEFT_NAME; - - /** Name of the position sensor of the right motor in the robot simulation. */ - static const char* POS_SENSOR_RIGHT_NAME; - - /** Name of the light sensor 0 in the robot simulation. */ - static const char* LIGHT_SENSOR_0_NAME; - - /** Name of the light sensor 1 in the robot simulation. */ - static const char* LIGHT_SENSOR_1_NAME; - - /** Name of the light sensor 2 in the robot simulation. */ - static const char* LIGHT_SENSOR_2_NAME; - - /** Name of the light sensor 3 in the robot simulation. */ - static const char* LIGHT_SENSOR_3_NAME; - - /** Name of the light sensor 4 in the robot simulation. */ - static const char* LIGHT_SENSOR_4_NAME; - - /** Name of the red LED in the robot simulation. */ - static const char* LED_RED_NAME; - - /** Name of the yellow LED in the robot simulation. */ - static const char* LED_YELLOW_NAME; - - /** Name of the green LED in the robot simulation. */ - static const char* LED_GREEN_NAME; - - /** Name of the front proximity sensor in the robot simulation. */ - static const char* PROXIMITY_SENSOR_FRONT_LEFT_NAME; - - /** Name of the front right proximity sensor in the robot simulation. */ - static const char* PROXIMITY_SENSOR_FRONT_RIGHT_NAME; /** Simulated roboter instance. */ webots::Robot m_robot; @@ -341,6 +280,9 @@ class Board : public IBoard /** Proximity sensors */ ProximitySensors m_proximitySensors; + /** Simulation serial driver */ + WebotsSerialDrv m_serialDrv; + /** Settings */ Settings m_settings; @@ -376,6 +318,16 @@ class Board : public IBoard return m_keyboard; } + /** + * Get the simulation serial driver, which is connected within Webots. + * + * @return If serial driver is available, it will return a pointer to it, otherwise nullptr. + */ + WebotsSerialDrv* getSimSerial() + { + return &m_serialDrv; + } + /** * Enable all simulation devices. * It is called by the main entry only. diff --git a/lib/HALLineFollowerSim/Board.cpp b/lib/HALLineFollowerSim/Board.cpp index 285f6f8b..d70999ad 100644 --- a/lib/HALLineFollowerSim/Board.cpp +++ b/lib/HALLineFollowerSim/Board.cpp @@ -33,6 +33,7 @@ * Includes *****************************************************************************/ #include +#include /****************************************************************************** * Compiler Switches @@ -54,68 +55,6 @@ * Local Variables *****************************************************************************/ -/* Name of the speaker in the robot simulation. */ -const char* Board::SPEAKER_NAME = "speaker"; - -/* Name of the display in the robot simulation. */ -const char* Board::DISPLAY_NAME = "robot_display"; - -/* Name of the left motor in the robot simulation. */ -const char* Board::LEFT_MOTOR_NAME = "motor_left"; - -/* Name of the right motor in the robot simulation. */ -const char* Board::RIGHT_MOTOR_NAME = "motor_right"; - -/* Name of the infrared emitter 0 in the robot simulation. */ -const char* Board::EMITTER_0_NAME = "emitter_l"; - -/* Name of the infrared emitter 1 in the robot simulation. */ -const char* Board::EMITTER_1_NAME = "emitter_lm"; - -/* Name of the infrared emitter 2 in the robot simulation. */ -const char* Board::EMITTER_2_NAME = "emitter_m"; - -/* Name of the infrared emitter 3 in the robot simulation. */ -const char* Board::EMITTER_3_NAME = "emitter_rm"; - -/* Name of the infrared emitter 4 in the robot simulation. */ -const char* Board::EMITTER_4_NAME = "emitter_r"; - -/** Name of the position sensor of the left motor in the robot simulation. */ -const char* Board::POS_SENSOR_LEFT_NAME = "position_sensor_left"; - -/** Name of the position sensor of the right motor in the robot simulation. */ -const char* Board::POS_SENSOR_RIGHT_NAME = "position_sensor_right"; - -/* Name of the light sensor 0 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_0_NAME = "lightsensor_l"; - -/* Name of the light sensor 1 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_1_NAME = "lightsensor_lm"; - -/* Name of the light sensor 2 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_2_NAME = "lightsensor_m"; - -/* Name of the light sensor 3 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_3_NAME = "lightsensor_rm"; - -/* Name of the light sensor 4 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_4_NAME = "lightsensor_r"; - -/* Name of the red LED in the robot simulation. */ -const char* Board::LED_RED_NAME = "led_red"; - -/* Name of the yellow LED in the robot simulation. */ -const char* Board::LED_YELLOW_NAME = "led_yellow"; - -/* Name of the green LED in the robot simulation. */ -const char* Board::LED_GREEN_NAME = "led_green"; - -#ifdef DEBUG_ODOMETRY -/* Name of the sender to the webots supervisor. */ -const char* Board::SENDER_NAME = "serialComTx"; -#endif /* DEBUG_ODOMETRY */ - /****************************************************************************** * Public Methods *****************************************************************************/ @@ -144,18 +83,23 @@ Board::Board() : m_buttonA(m_keyboard), m_buttonB(m_keyboard), m_buttonC(m_keyboard), - m_buzzer(m_robot.getSpeaker(SPEAKER_NAME)), - m_display(m_robot.getDisplay(DISPLAY_NAME)), - m_encoders(m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME), m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)), - m_lineSensors(m_robot.getEmitter(EMITTER_0_NAME), m_robot.getEmitter(EMITTER_1_NAME), - m_robot.getEmitter(EMITTER_2_NAME), m_robot.getEmitter(EMITTER_3_NAME), - m_robot.getEmitter(EMITTER_4_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)), - m_motors(m_robot.getMotor(LEFT_MOTOR_NAME), m_robot.getMotor(RIGHT_MOTOR_NAME)), - m_ledRed(m_robot.getLED(LED_RED_NAME)), - m_ledYellow(m_robot.getLED(LED_YELLOW_NAME)), - m_ledGreen(m_robot.getLED(LED_GREEN_NAME)), + m_buzzer(m_robot.getSpeaker(RobotDeviceNames::SPEAKER_NAME)), + m_display(m_robot.getDisplay(RobotDeviceNames::DISPLAY_NAME)), + m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), + m_lineSensors( + m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), + m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), + m_ledRed(m_robot.getLED(RobotDeviceNames::LED_RED_NAME)), + m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), + m_ledGreen(m_robot.getLED(RobotDeviceNames::LED_GREEN_NAME)), m_settings() #ifdef DEBUG_ODOMETRY , @@ -169,13 +113,13 @@ void Board::enableSimulationDevices() const int timeStep = m_simTime.getTimeStep(); m_robot.getKeyboard()->enable(timeStep); - m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME)->enable(timeStep); - m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); } /****************************************************************************** diff --git a/lib/HALLineFollowerSim/Board.h b/lib/HALLineFollowerSim/Board.h index eb439e40..992e5147 100644 --- a/lib/HALLineFollowerSim/Board.h +++ b/lib/HALLineFollowerSim/Board.h @@ -61,6 +61,7 @@ #include #include #include +#include #ifdef DEBUG_ODOMETRY #include @@ -240,73 +241,6 @@ class Board : public IBoard } private: - /** Name of the speaker in the robot simulation. */ - static const char* SPEAKER_NAME; - - /** Name of the display in the robot simulation. */ - static const char* DISPLAY_NAME; - - /** Name of the left motor in the robot simulation. */ - static const char* LEFT_MOTOR_NAME; - - /** Name of the right motor in the robot simulation. */ - static const char* RIGHT_MOTOR_NAME; - - /** Name of the infrared emitter 0 in the robot simulation. */ - static const char* EMITTER_0_NAME; - - /** Name of the infrared emitter 1 in the robot simulation. */ - static const char* EMITTER_1_NAME; - - /** Name of the infrared emitter 2 in the robot simulation. */ - static const char* EMITTER_2_NAME; - - /** Name of the infrared emitter 3 in the robot simulation. */ - static const char* EMITTER_3_NAME; - - /** Name of the infrared emitter 4 in the robot simulation. */ - static const char* EMITTER_4_NAME; - - /** Name of the position sensor of the left motor in the robot simulation. */ - static const char* POS_SENSOR_LEFT_NAME; - - /** Name of the position sensor of the right motor in the robot simulation. */ - static const char* POS_SENSOR_RIGHT_NAME; - - /** Name of the light sensor 0 in the robot simulation. */ - static const char* LIGHT_SENSOR_0_NAME; - - /** Name of the light sensor 1 in the robot simulation. */ - static const char* LIGHT_SENSOR_1_NAME; - - /** Name of the light sensor 2 in the robot simulation. */ - static const char* LIGHT_SENSOR_2_NAME; - - /** Name of the light sensor 3 in the robot simulation. */ - static const char* LIGHT_SENSOR_3_NAME; - - /** Name of the light sensor 4 in the robot simulation. */ - static const char* LIGHT_SENSOR_4_NAME; - - /** Name of the red LED in the robot simulation. */ - static const char* LED_RED_NAME; - - /** Name of the yellow LED in the robot simulation. */ - static const char* LED_YELLOW_NAME; - - /** Name of the green LED in the robot simulation. */ - static const char* LED_GREEN_NAME; - - /** Name of the front proximity sensor in the robot simulation. */ - static const char* PROXIMITY_SENSOR_FRONT_LEFT_NAME; - - /** Name of the front right proximity sensor in the robot simulation. */ - static const char* PROXIMITY_SENSOR_FRONT_RIGHT_NAME; - -#ifdef DEBUG_ODOMETRY - /** Name of the sender to the webots supervisor. */ - static const char* SENDER_NAME; -#endif /* DEBUG_ODOMETRY */ /** Simulated roboter instance. */ webots::Robot m_robot; @@ -392,6 +326,17 @@ class Board : public IBoard return m_keyboard; } + /** + * Get the simulation serial driver, which is connected within Webots. + * + * @return If serial driver is available, it will return a pointer to it, otherwise nullptr. + */ + WebotsSerialDrv* getSimSerial() + { + /* Not supported. */ + return nullptr; + } + /** * Enable all simulation devices. * It is called by the main entry only. diff --git a/lib/HALRemoteControlSim/Board.cpp b/lib/HALRemoteControlSim/Board.cpp index 21a8f080..11ce3e23 100644 --- a/lib/HALRemoteControlSim/Board.cpp +++ b/lib/HALRemoteControlSim/Board.cpp @@ -33,6 +33,7 @@ * Includes *****************************************************************************/ #include +#include /****************************************************************************** * Compiler Switches @@ -54,69 +55,6 @@ * Local Variables *****************************************************************************/ -/* Name of the speaker in the robot simulation. */ -const char* Board::SPEAKER_NAME = "speaker"; - -/* Name of the display in the robot simulation. */ -const char* Board::DISPLAY_NAME = "robot_display"; - -/* Name of the left motor in the robot simulation. */ -const char* Board::LEFT_MOTOR_NAME = "motor_left"; - -/* Name of the right motor in the robot simulation. */ -const char* Board::RIGHT_MOTOR_NAME = "motor_right"; - -/* Name of the infrared emitter 0 in the robot simulation. */ -const char* Board::EMITTER_0_NAME = "emitter_l"; - -/* Name of the infrared emitter 1 in the robot simulation. */ -const char* Board::EMITTER_1_NAME = "emitter_lm"; - -/* Name of the infrared emitter 2 in the robot simulation. */ -const char* Board::EMITTER_2_NAME = "emitter_m"; - -/* Name of the infrared emitter 3 in the robot simulation. */ -const char* Board::EMITTER_3_NAME = "emitter_rm"; - -/* Name of the infrared emitter 4 in the robot simulation. */ -const char* Board::EMITTER_4_NAME = "emitter_r"; - -/** Name of the position sensor of the left motor in the robot simulation. */ -const char* Board::POS_SENSOR_LEFT_NAME = "position_sensor_left"; - -/** Name of the position sensor of the right motor in the robot simulation. */ -const char* Board::POS_SENSOR_RIGHT_NAME = "position_sensor_right"; - -/* Name of the light sensor 0 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_0_NAME = "lightsensor_l"; - -/* Name of the light sensor 1 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_1_NAME = "lightsensor_lm"; - -/* Name of the light sensor 2 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_2_NAME = "lightsensor_m"; - -/* Name of the light sensor 3 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_3_NAME = "lightsensor_rm"; - -/* Name of the light sensor 4 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_4_NAME = "lightsensor_r"; - -/* Name of the red LED in the robot simulation. */ -const char* Board::LED_RED_NAME = "led_red"; - -/* Name of the yellow LED in the robot simulation. */ -const char* Board::LED_YELLOW_NAME = "led_yellow"; - -/* Name of the green LED in the robot simulation. */ -const char* Board::LED_GREEN_NAME = "led_green"; - -/* Name of the front left proximity sensor in the robot simulation. */ -const char* Board::PROXIMITY_SENSOR_FRONT_LEFT_NAME = "proxim_sensor_fl"; - -/* Name of the front right proximity sensor in the robot simulation. */ -const char* Board::PROXIMITY_SENSOR_FRONT_RIGHT_NAME = "proxim_sensor_fr"; - /****************************************************************************** * Public Methods *****************************************************************************/ @@ -146,20 +84,27 @@ Board::Board() : m_buttonA(m_keyboard), m_buttonB(m_keyboard), m_buttonC(m_keyboard), - m_buzzer(m_robot.getSpeaker(SPEAKER_NAME)), - m_display(m_robot.getDisplay(DISPLAY_NAME)), - m_encoders(m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME), m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)), - m_lineSensors(m_robot.getEmitter(EMITTER_0_NAME), m_robot.getEmitter(EMITTER_1_NAME), - m_robot.getEmitter(EMITTER_2_NAME), m_robot.getEmitter(EMITTER_3_NAME), - m_robot.getEmitter(EMITTER_4_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)), - m_motors(m_robot.getMotor(LEFT_MOTOR_NAME), m_robot.getMotor(RIGHT_MOTOR_NAME)), - m_ledRed(m_robot.getLED(LED_RED_NAME)), - m_ledYellow(m_robot.getLED(LED_YELLOW_NAME)), - m_ledGreen(m_robot.getLED(LED_GREEN_NAME)), - m_proximitySensors(m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_LEFT_NAME), - m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_RIGHT_NAME)), + m_buzzer(m_robot.getSpeaker(RobotDeviceNames::SPEAKER_NAME)), + m_display(m_robot.getDisplay(RobotDeviceNames::DISPLAY_NAME)), + m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), + m_lineSensors( + m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), + m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), + m_ledRed(m_robot.getLED(RobotDeviceNames::LED_RED_NAME)), + m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), + m_ledGreen(m_robot.getLED(RobotDeviceNames::LED_GREEN_NAME)), + m_proximitySensors(m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)), + m_serialDrv(m_robot.getEmitter(RobotDeviceNames::EMITTER_NAME_SERIAL), + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)), m_settings() { } @@ -169,15 +114,16 @@ void Board::enableSimulationDevices() const int timeStep = m_simTime.getTimeStep(); m_robot.getKeyboard()->enable(timeStep); - m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME)->enable(timeStep); - m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)->enable(timeStep); - m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_LEFT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_RIGHT_NAME)->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_LEFT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::PROXIMITY_SENSOR_FRONT_RIGHT_NAME)->enable(timeStep); + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)->enable(timeStep); } /****************************************************************************** * External Functions diff --git a/lib/HALRemoteControlSim/Board.h b/lib/HALRemoteControlSim/Board.h index 50b27ee4..1ee66215 100644 --- a/lib/HALRemoteControlSim/Board.h +++ b/lib/HALRemoteControlSim/Board.h @@ -62,6 +62,7 @@ #include #include #include +#include /****************************************************************************** * Macros @@ -233,68 +234,6 @@ class Board : public IBoard } private: - /** Name of the speaker in the robot simulation. */ - static const char* SPEAKER_NAME; - - /** Name of the display in the robot simulation. */ - static const char* DISPLAY_NAME; - - /** Name of the left motor in the robot simulation. */ - static const char* LEFT_MOTOR_NAME; - - /** Name of the right motor in the robot simulation. */ - static const char* RIGHT_MOTOR_NAME; - - /** Name of the infrared emitter 0 in the robot simulation. */ - static const char* EMITTER_0_NAME; - - /** Name of the infrared emitter 1 in the robot simulation. */ - static const char* EMITTER_1_NAME; - - /** Name of the infrared emitter 2 in the robot simulation. */ - static const char* EMITTER_2_NAME; - - /** Name of the infrared emitter 3 in the robot simulation. */ - static const char* EMITTER_3_NAME; - - /** Name of the infrared emitter 4 in the robot simulation. */ - static const char* EMITTER_4_NAME; - - /** Name of the position sensor of the left motor in the robot simulation. */ - static const char* POS_SENSOR_LEFT_NAME; - - /** Name of the position sensor of the right motor in the robot simulation. */ - static const char* POS_SENSOR_RIGHT_NAME; - - /** Name of the light sensor 0 in the robot simulation. */ - static const char* LIGHT_SENSOR_0_NAME; - - /** Name of the light sensor 1 in the robot simulation. */ - static const char* LIGHT_SENSOR_1_NAME; - - /** Name of the light sensor 2 in the robot simulation. */ - static const char* LIGHT_SENSOR_2_NAME; - - /** Name of the light sensor 3 in the robot simulation. */ - static const char* LIGHT_SENSOR_3_NAME; - - /** Name of the light sensor 4 in the robot simulation. */ - static const char* LIGHT_SENSOR_4_NAME; - - /** Name of the red LED in the robot simulation. */ - static const char* LED_RED_NAME; - - /** Name of the yellow LED in the robot simulation. */ - static const char* LED_YELLOW_NAME; - - /** Name of the green LED in the robot simulation. */ - static const char* LED_GREEN_NAME; - - /** Name of the front proximity sensor in the robot simulation. */ - static const char* PROXIMITY_SENSOR_FRONT_LEFT_NAME; - - /** Name of the front right proximity sensor in the robot simulation. */ - static const char* PROXIMITY_SENSOR_FRONT_RIGHT_NAME; /** Simulated roboter instance. */ webots::Robot m_robot; @@ -341,6 +280,9 @@ class Board : public IBoard /** Proximity sensors */ ProximitySensors m_proximitySensors; + /** Simulation serial driver */ + WebotsSerialDrv m_serialDrv; + /** Settings */ Settings m_settings; @@ -376,6 +318,16 @@ class Board : public IBoard return m_keyboard; } + /** + * Get the simulation serial driver, which is connected within Webots. + * + * @return If serial driver is available, it will return a pointer to it, otherwise nullptr. + */ + WebotsSerialDrv* getSimSerial() + { + return &m_serialDrv; + } + /** * Enable all simulation devices. * It is called by the main entry only. diff --git a/lib/HALSensorFusionSim/Board.cpp b/lib/HALSensorFusionSim/Board.cpp index 6d00879d..e55cc70e 100644 --- a/lib/HALSensorFusionSim/Board.cpp +++ b/lib/HALSensorFusionSim/Board.cpp @@ -33,6 +33,7 @@ * Includes *****************************************************************************/ #include +#include /****************************************************************************** * Compiler Switches @@ -54,63 +55,6 @@ * Local Variables *****************************************************************************/ -/* Name of the speaker in the robot simulation. */ -const char* Board::SPEAKER_NAME = "speaker"; - -/* Name of the left motor in the robot simulation. */ -const char* Board::LEFT_MOTOR_NAME = "motor_left"; - -/* Name of the right motor in the robot simulation. */ -const char* Board::RIGHT_MOTOR_NAME = "motor_right"; - -/* Name of the infrared emitter 0 in the robot simulation. */ -const char* Board::EMITTER_0_NAME = "emitter_l"; - -/* Name of the infrared emitter 1 in the robot simulation. */ -const char* Board::EMITTER_1_NAME = "emitter_lm"; - -/* Name of the infrared emitter 2 in the robot simulation. */ -const char* Board::EMITTER_2_NAME = "emitter_m"; - -/* Name of the infrared emitter 3 in the robot simulation. */ -const char* Board::EMITTER_3_NAME = "emitter_rm"; - -/* Name of the infrared emitter 4 in the robot simulation. */ -const char* Board::EMITTER_4_NAME = "emitter_r"; - -/** Name of the position sensor of the left motor in the robot simulation. */ -const char* Board::POS_SENSOR_LEFT_NAME = "position_sensor_left"; - -/** Name of the position sensor of the right motor in the robot simulation. */ -const char* Board::POS_SENSOR_RIGHT_NAME = "position_sensor_right"; - -/* Name of the light sensor 0 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_0_NAME = "lightsensor_l"; - -/* Name of the light sensor 1 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_1_NAME = "lightsensor_lm"; - -/* Name of the light sensor 2 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_2_NAME = "lightsensor_m"; - -/* Name of the light sensor 3 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_3_NAME = "lightsensor_rm"; - -/* Name of the light sensor 4 in the robot simulation. */ -const char* Board::LIGHT_SENSOR_4_NAME = "lightsensor_r"; - -/* Name of the yellow LED in the robot simulation. */ -const char* Board::LED_YELLOW_NAME = "led_yellow"; - -/* Name of the accelerometer in the robot simulation. */ -const char* Board::ACCELEROMETER_NAME = "accelerometer"; - -/* Name of the gyro in the robot simulation. */ -const char* Board::GYRO_NAME = "gyro"; - -/* Name of the magnetometer in the robot simulation. */ -const char* Board::MAGNETOMETER_NAME = "magnetometer"; - /****************************************************************************** * Public Methods *****************************************************************************/ @@ -141,17 +85,24 @@ Board::Board() : m_simTime(m_robot), m_keyboard(m_simTime, m_robot.getKeyboard()), m_buttonA(m_keyboard), - m_buzzer(m_robot.getSpeaker(SPEAKER_NAME)), - m_encoders(m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME), m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)), - m_lineSensors(m_robot.getEmitter(EMITTER_0_NAME), m_robot.getEmitter(EMITTER_1_NAME), - m_robot.getEmitter(EMITTER_2_NAME), m_robot.getEmitter(EMITTER_3_NAME), - m_robot.getEmitter(EMITTER_4_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME), - m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME), m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)), - m_motors(m_robot.getMotor(LEFT_MOTOR_NAME), m_robot.getMotor(RIGHT_MOTOR_NAME)), - m_ledYellow(m_robot.getLED(LED_YELLOW_NAME)), - m_imu(m_robot.getAccelerometer(ACCELEROMETER_NAME), m_robot.getGyro(GYRO_NAME), - m_robot.getCompass(MAGNETOMETER_NAME)), + m_buzzer(m_robot.getSpeaker(RobotDeviceNames::SPEAKER_NAME)), + m_encoders(m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME), + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)), + m_lineSensors( + m_robot.getEmitter(RobotDeviceNames::EMITTER_0_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_1_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_2_NAME), m_robot.getEmitter(RobotDeviceNames::EMITTER_3_NAME), + m_robot.getEmitter(RobotDeviceNames::EMITTER_4_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME), + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)), + m_motors(m_robot.getMotor(RobotDeviceNames::LEFT_MOTOR_NAME), m_robot.getMotor(RobotDeviceNames::RIGHT_MOTOR_NAME)), + m_ledYellow(m_robot.getLED(RobotDeviceNames::LED_YELLOW_NAME)), + m_imu(m_robot.getAccelerometer(RobotDeviceNames::ACCELEROMETER_NAME), m_robot.getGyro(RobotDeviceNames::GYRO_NAME), + m_robot.getCompass(RobotDeviceNames::MAGNETOMETER_NAME)), + m_serialDrv(m_robot.getEmitter(RobotDeviceNames::EMITTER_NAME_SERIAL), + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)), m_settings() { } @@ -161,16 +112,17 @@ void Board::enableSimulationDevices() const int timeStep = m_simTime.getTimeStep(); m_robot.getKeyboard()->enable(timeStep); - m_robot.getPositionSensor(POS_SENSOR_LEFT_NAME)->enable(timeStep); - m_robot.getPositionSensor(POS_SENSOR_RIGHT_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_0_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_1_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_2_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_3_NAME)->enable(timeStep); - m_robot.getDistanceSensor(LIGHT_SENSOR_4_NAME)->enable(timeStep); - m_robot.getAccelerometer(ACCELEROMETER_NAME)->enable(timeStep); - m_robot.getGyro(GYRO_NAME)->enable(timeStep); - m_robot.getCompass(MAGNETOMETER_NAME)->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_LEFT_NAME)->enable(timeStep); + m_robot.getPositionSensor(RobotDeviceNames::POS_SENSOR_RIGHT_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_0_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_1_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_2_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_3_NAME)->enable(timeStep); + m_robot.getDistanceSensor(RobotDeviceNames::LIGHT_SENSOR_4_NAME)->enable(timeStep); + m_robot.getAccelerometer(RobotDeviceNames::ACCELEROMETER_NAME)->enable(timeStep); + m_robot.getGyro(RobotDeviceNames::GYRO_NAME)->enable(timeStep); + m_robot.getCompass(RobotDeviceNames::MAGNETOMETER_NAME)->enable(timeStep); + m_robot.getReceiver(RobotDeviceNames::RECEIVER_NAME_SERIAL)->enable(timeStep); } /****************************************************************************** diff --git a/lib/HALSensorFusionSim/Board.h b/lib/HALSensorFusionSim/Board.h index 70a5ae61..f9f714b0 100644 --- a/lib/HALSensorFusionSim/Board.h +++ b/lib/HALSensorFusionSim/Board.h @@ -57,6 +57,7 @@ #include #include #include +#include /****************************************************************************** * Macros @@ -178,62 +179,6 @@ class Board : public IBoard } private: - /** Name of the speaker in the robot simulation. */ - static const char* SPEAKER_NAME; - - /** Name of the left motor in the robot simulation. */ - static const char* LEFT_MOTOR_NAME; - - /** Name of the right motor in the robot simulation. */ - static const char* RIGHT_MOTOR_NAME; - - /** Name of the infrared emitter 0 in the robot simulation. */ - static const char* EMITTER_0_NAME; - - /** Name of the infrared emitter 1 in the robot simulation. */ - static const char* EMITTER_1_NAME; - - /** Name of the infrared emitter 2 in the robot simulation. */ - static const char* EMITTER_2_NAME; - - /** Name of the infrared emitter 3 in the robot simulation. */ - static const char* EMITTER_3_NAME; - - /** Name of the infrared emitter 4 in the robot simulation. */ - static const char* EMITTER_4_NAME; - - /** Name of the position sensor of the left motor in the robot simulation. */ - static const char* POS_SENSOR_LEFT_NAME; - - /** Name of the position sensor of the right motor in the robot simulation. */ - static const char* POS_SENSOR_RIGHT_NAME; - - /** Name of the light sensor 0 in the robot simulation. */ - static const char* LIGHT_SENSOR_0_NAME; - - /** Name of the light sensor 1 in the robot simulation. */ - static const char* LIGHT_SENSOR_1_NAME; - - /** Name of the light sensor 2 in the robot simulation. */ - static const char* LIGHT_SENSOR_2_NAME; - - /** Name of the light sensor 3 in the robot simulation. */ - static const char* LIGHT_SENSOR_3_NAME; - - /** Name of the light sensor 4 in the robot simulation. */ - static const char* LIGHT_SENSOR_4_NAME; - - /** Name of the yellow LED in the robot simulation. */ - static const char* LED_YELLOW_NAME; - - /** Name of the accelerometer in the robot simulation. */ - static const char* ACCELEROMETER_NAME; - - /** Name of the gyro in the robot simulation. */ - static const char* GYRO_NAME; - - /** Name of the Magnetometer in the robot simulation. */ - static const char* MAGNETOMETER_NAME; /** Simulated roboter instance. */ webots::Robot m_robot; @@ -265,6 +210,9 @@ class Board : public IBoard /** IMU driver */ IMU m_imu; + /** Simulation serial driver */ + WebotsSerialDrv m_serialDrv; + /** Settings */ Settings m_settings; @@ -300,6 +248,16 @@ class Board : public IBoard return m_keyboard; } + /** + * Get the simulation serial driver, which is connected within Webots. + * + * @return If serial driver is available, it will return a pointer to it, otherwise nullptr. + */ + WebotsSerialDrv* getSimSerial() + { + return &m_serialDrv; + } + /** * Enable all simulation devices. * It is called by the main entry only. diff --git a/lib/HALSim/RobotDeviceNames.h b/lib/HALSim/RobotDeviceNames.h new file mode 100644 index 00000000..a6619367 --- /dev/null +++ b/lib/HALSim/RobotDeviceNames.h @@ -0,0 +1,146 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Robot device names in Webots + * @author Andreas Merkle + * + * @addtogroup HALInterfaces + * + * @{ + */ + +#ifndef ROBOT_DEVICE_NAMES_H +#define ROBOT_DEVICE_NAMES_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * Robot device names in Webots. + */ +namespace RobotDeviceNames +{ + /** Name of the speaker in the robot simulation. */ + static const char* SPEAKER_NAME = "speaker"; + + /** Name of the display in the robot simulation. */ + static const char* DISPLAY_NAME = "robot_display"; + + /** Name of the left motor in the robot simulation. */ + static const char* LEFT_MOTOR_NAME = "motor_left"; + + /** Name of the right motor in the robot simulation. */ + static const char* RIGHT_MOTOR_NAME = "motor_right"; + + /** Name of the infrared emitter 0 in the robot simulation. */ + static const char* EMITTER_0_NAME = "emitter_l"; + + /** Name of the infrared emitter 1 in the robot simulation. */ + static const char* EMITTER_1_NAME = "emitter_lm"; + + /** Name of the infrared emitter 2 in the robot simulation. */ + static const char* EMITTER_2_NAME = "emitter_m"; + + /** Name of the infrared emitter 3 in the robot simulation. */ + static const char* EMITTER_3_NAME = "emitter_rm"; + + /** Name of the infrared emitter 4 in the robot simulation. */ + static const char* EMITTER_4_NAME = "emitter_r"; + + /** Name of the position sensor of the left motor in the robot simulation. */ + static const char* POS_SENSOR_LEFT_NAME = "position_sensor_left"; + + /** Name of the position sensor of the right motor in the robot simulation. */ + static const char* POS_SENSOR_RIGHT_NAME = "position_sensor_right"; + + /** Name of the light sensor 0 in the robot simulation. */ + static const char* LIGHT_SENSOR_0_NAME = "lightsensor_l"; + + /** Name of the light sensor 1 in the robot simulation. */ + static const char* LIGHT_SENSOR_1_NAME = "lightsensor_lm"; + + /** Name of the light sensor 2 in the robot simulation. */ + static const char* LIGHT_SENSOR_2_NAME = "lightsensor_m"; + + /** Name of the light sensor 3 in the robot simulation. */ + static const char* LIGHT_SENSOR_3_NAME = "lightsensor_rm"; + + /** Name of the light sensor 4 in the robot simulation. */ + static const char* LIGHT_SENSOR_4_NAME = "lightsensor_r"; + + /** Name of the red LED in the robot simulation. */ + static const char* LED_RED_NAME = "led_red"; + + /** Name of the yellow LED in the robot simulation. */ + static const char* LED_YELLOW_NAME = "led_yellow"; + + /** Name of the green LED in the robot simulation. */ + static const char* LED_GREEN_NAME = "led_green"; + + /** Name of the front left proximity sensor in the robot simulation. */ + static const char* PROXIMITY_SENSOR_FRONT_LEFT_NAME = "proxim_sensor_fl"; + + /** Name of the front right proximity sensor in the robot simulation. */ + static const char* PROXIMITY_SENSOR_FRONT_RIGHT_NAME = "proxim_sensor_fr"; + + /** Name of the accelerometer in the robot simulation. */ + static const char* ACCELEROMETER_NAME = "accelerometer"; + + /** Name of the gyro in the robot simulation. */ + static const char* GYRO_NAME = "gyro"; + + /** Name of the magnetometer in the robot simulation. */ + static const char* MAGNETOMETER_NAME = "magnetometer"; + + /** Name of the serial emitter in the RadonUlzer simulation. */ + static const char* EMITTER_NAME_SERIAL = "serialComTx"; + + /** Name of the serial receiver in the RadonUlzer simulation. */ + static const char* RECEIVER_NAME_SERIAL = "serialComRx"; + +}; /* namespace RobotDeviceNames */ + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* ROBOT_DEVICE_NAMES_H */ +/** @} */ diff --git a/lib/HALSim/WebotsSerialDrv.cpp b/lib/HALSim/WebotsSerialDrv.cpp new file mode 100644 index 00000000..24d01752 --- /dev/null +++ b/lib/HALSim/WebotsSerialDrv.cpp @@ -0,0 +1,309 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Webots serial driver + * @author Andreas Merkle + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "WebotsSerialDrv.h" +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +WebotsSerialDrv::WebotsSerialDrv(webots::Emitter* emitter, webots::Receiver* receiver) : + Stream(), + m_emitter(emitter), + m_receiver(receiver), + m_dataRemains(0U) +{ + /* Nothing to do. */ +} + +WebotsSerialDrv::~WebotsSerialDrv() +{ + /* Nothing to do. */ +} + +void WebotsSerialDrv::setRxChannel(int32_t channelId) +{ + if (nullptr != m_receiver) + { + /* Shall be positive for inter-robot communication. */ + if (0 <= channelId) + { + m_receiver->setChannel(channelId); + } + } +} + +void WebotsSerialDrv::setTxChannel(int32_t channelId) +{ + if (nullptr != m_emitter) + { + /* Shall be positive for inter-robot communication. */ + if (0 <= channelId) + { + m_emitter->setChannel(channelId); + } + } +} + +void WebotsSerialDrv::print(const char str[]) +{ + if (nullptr != str) + { + (void)m_emitter->send(str, strlen(str)); + } +} + +void WebotsSerialDrv::print(uint8_t value) +{ + char buffer[4U]; /* [0; 255] + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%u", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::print(uint16_t value) +{ + char buffer[6U]; /* [0; 65535] + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%u", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::print(uint32_t value) +{ + char buffer[11U]; /* [0; 4294967295] + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%u", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::print(int8_t value) +{ + char buffer[5U]; /* [-128; 127] + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%d", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::print(int16_t value) +{ + char buffer[6U]; /* [-32768; 32767] + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%d", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::print(int32_t value) +{ + char buffer[12U]; /* [-2147483648; 2147483647] + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%d", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::println(const char str[]) +{ + if (nullptr != str) + { + size_t length = strlen(str); + char buffer[length + 1U]; + + length = snprintf(buffer, sizeof(buffer), "%s\n", str); + + (void)m_emitter->send(buffer, length); + } +} + +void WebotsSerialDrv::println(uint8_t value) +{ + char buffer[5U]; /* [0; 255] + LF + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%u\n", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::println(uint16_t value) +{ + char buffer[7U]; /* [0; 65535] + LF + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%u\n", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::println(uint32_t value) +{ + char buffer[12U]; /* [0; 4294967295] + LF + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%u\n", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::println(int8_t value) +{ + char buffer[6U]; /* [-128; 127] + LF + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%d\n", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::println(int16_t value) +{ + char buffer[7U]; /* [-32768; 32767] + LF + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%d\n", value); + + (void)m_emitter->send(buffer, length); +} + +void WebotsSerialDrv::println(int32_t value) +{ + char buffer[13U]; /* [-2147483648; 2147483647] + LF + string termination */ + int32_t length = snprintf(buffer, sizeof(buffer), "%d\n", value); + + (void)m_emitter->send(buffer, length); +} + +size_t WebotsSerialDrv::write(const uint8_t* buffer, size_t length) +{ + int32_t result = m_emitter->send(buffer, length); + const int32_t SUCCESS = 1; + + return (SUCCESS == result) ? length : 0; +} + +int WebotsSerialDrv::available() const +{ + int32_t dataSize = 0; + + /* If in the last read not all data was read, return the remaining bytes. */ + if (0U < m_dataRemains) + { + dataSize = m_dataRemains; + } + /* Otherwise return the size of the current head packet. + * Attention, calling the getDataSize() is illegal in case the queue is + * empty! + */ + else if (0 < m_receiver->getQueueLength()) + { + dataSize = m_receiver->getDataSize(); + } + else + { + /* Nothing to do. */ + ; + } + + return dataSize; +} + +size_t WebotsSerialDrv::readBytes(uint8_t* buffer, size_t length) +{ + size_t read = 0U; + + /* Attention, calling the getData() or getDataSize() is illegal in case the + * queue is empty! + */ + if (0 < m_receiver->getQueueLength()) + { + const uint8_t* message = static_cast(m_receiver->getData()); + int32_t messageSize = m_receiver->getDataSize(); + + /* If in the last read not all data was read, return the remaining bytes. */ + if (0U < m_dataRemains) + { + /* Partial read? */ + if (m_dataRemains > length) + { + read = length; + } + /* Read all. */ + else + { + read = m_dataRemains; + } + + (void)memcpy(buffer, &message[messageSize - m_dataRemains], read); + m_dataRemains -= read; + } + else + { + /* Partial read? */ + if (messageSize > length) + { + read = length; + } + /* Read all. */ + else + { + read = messageSize; + } + + (void)memcpy(buffer, message, read); + m_dataRemains = messageSize - read; + } + + /* If the complete head packet is read, it will be thrown away. */ + if (0U == m_dataRemains) + { + m_receiver->nextPacket(); + } + } + + return read; +} + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/ArduinoNative/SocketServer.h b/lib/HALSim/WebotsSerialDrv.h similarity index 52% rename from lib/ArduinoNative/SocketServer.h rename to lib/HALSim/WebotsSerialDrv.h index c063ac16..c5d8253c 100644 --- a/lib/ArduinoNative/SocketServer.h +++ b/lib/HALSim/WebotsSerialDrv.h @@ -25,18 +25,23 @@ DESCRIPTION *******************************************************************************/ /** - * @brief Socket Server for Inter-Process Communication - * @author Gabryel Reyes + * @brief Webots serial driver + * @author Andreas Merkle + * + * @addtogroup HALSim + * + * @{ */ -#ifndef SOCKET_SERVER_H -#define SOCKET_SERVER_H +#ifndef WEBOTS_SERIAL_DRV_H +#define WEBOTS_SERIAL_DRV_H /****************************************************************************** * Includes *****************************************************************************/ - #include "Stream.h" +#include +#include /****************************************************************************** * Macros @@ -47,170 +52,180 @@ *****************************************************************************/ /** - * Socket Server for Inter-Process Communication. + * Webots serial driver. */ -class SocketServer : public Stream +class WebotsSerialDrv : public Stream { public: /** - * Construct a SocketServer. + * Construct a Webots serial driver. + * + * @param[in] emitter Webots emitter used to send data over simulated serial. + * @param[in] receiver Webots receiver used to receive data over simulated serial. + */ + WebotsSerialDrv(webots::Emitter* emitter, webots::Receiver* receiver); + + /** + * Destruct the Webots serial driver. */ - SocketServer(); + virtual ~WebotsSerialDrv(); /** - * Destruct the SocketServer. + * Set the serial receive channel id. + * + * @param[in] channelId Channel ID, shall be positive for inter-robot communication. */ - ~SocketServer(); + void setRxChannel(int32_t channelId); /** - * Initialize the SocketServer. - * @param[in] port Port number to set the Socket to. - * @param[in] maxConnections Number of maxConnections allowed. - * @returns true if server has been succesfully set-up. + * Set the serial sender channel id. + * + * @param[in] channelId Channel ID, shall be positive for inter-robot communication. */ - bool init(const char* port, uint8_t maxConnections); + void setTxChannel(int32_t channelId); /** - * Print argument to the Output Stream. + * Print argument to the output stream. + * * @param[in] str Argument to print. */ void print(const char str[]) final; /** - * Print argument to the Output Stream. + * Print argument to the output stream. + * * @param[in] value Argument to print. */ void print(uint8_t value) final; /** - * Print argument to the Output Stream. + * Print argument to the output stream. + * * @param[in] value Argument to print. */ void print(uint16_t value) final; /** - * Print argument to the Output Stream. + * Print argument to the output stream. + * * @param[in] value Argument to print. */ void print(uint32_t value) final; /** - * Print argument to the Output Stream. + * Print argument to the output stream. + * * @param[in] value Argument to print. */ void print(int8_t value) final; /** - * Print argument to the Output Stream. + * Print argument to the output stream. + * * @param[in] value Argument to print. */ void print(int16_t value) final; /** - * Print argument to the Output Stream. + * Print argument to the output stream. + * * @param[in] value Argument to print. */ void print(int32_t value) final; /** - * Print argument to the Output Stream. - * Appends Carriage Return at the end of the argument. + * Print argument to the output stream. + * Appends carriage return at the end of the argument. + * * @param[in] str Argument to print. */ void println(const char str[]) final; /** - * Print argument to the Output Stream. - * Appends Carriage Return at the end of the argument. + * Print argument to the output stream. + * Appends carriage return at the end of the argument. + * * @param[in] value Argument to print. */ void println(uint8_t value) final; /** - * Print argument to the Output Stream. - * Appends Carriage Return at the end of the argument. + * Print argument to the output stream. + * Appends carriage return at the end of the argument. + * * @param[in] value Argument to print. */ void println(uint16_t value) final; /** - * Print argument to the Output Stream. - * Appends Carriage Return at the end of the argument. + * Print argument to the output stream. + * Appends carriage return at the end of the argument. + * * @param[in] value Argument to print. */ void println(uint32_t value) final; /** - * Print argument to the Output Stream. - * Appends Carriage Return at the end of the argument. + * Print argument to the output stream. + * Appends carriage return at the end of the argument. + * * @param[in] value Argument to print. */ void println(int8_t value) final; /** - * Print argument to the Output Stream. - * Appends Carriage Return at the end of the argument. + * Print argument to the output stream. + * Appends carriage return at the end of the argument. + * * @param[in] value Argument to print. */ void println(int16_t value) final; /** - * Print argument to the Output Stream. - * Appends Carriage Return at the end of the argument. + * Print argument to the output stream. + * Appends carriage return at the end of the argument. + * * @param[in] value Argument to print. */ void println(int32_t value) final; /** - * Send a message to the socket. - * @param[in] buf Byte buffer to send - * @param[in] length Number of bytes to send - * @returns Number of bytes written + * Write data buffer to serial. + * + * @param[in] buffer Byte buffer to send. + * @param[in] length Number of bytes to send. + * + * @return Number of bytes written. */ size_t write(const uint8_t* buffer, size_t length) final; /** * Check if any data has been received. - * @returns number of available bytes. + * + * @return Number of available bytes. */ int available() const final; /** - * Read bytes into a buffer. - * @param[in] buffer Array to write bytes to. - * @param[in] length number of bytes to be read. - * @returns Number of bytes read from Stream. + * Read data from serial. + * + * @param[in] buffer Array to read data in. + * @param[in] length Number of bytes to be read. + * + * @return Number of bytes read from stream. */ size_t readBytes(uint8_t* buffer, size_t length) final; - /** - * Process the receiving of messages and client connections. - */ - void process(); - private: - /** Struct for Implementation of PIMPL Idiom. */ - struct SocketServerImpl; - - /** SocketServer Members. PIMPL Idiom. */ - SocketServerImpl* m_members; + webots::Emitter* m_emitter; /**< The emitter used to send data. */ + webots::Receiver* m_receiver; /**< The receiver used to receive data. */ + size_t m_dataRemains; /**< Number of bytes which are remaining in head packet. */ /* Not allowed. */ - SocketServer(const SocketServer& srv); - SocketServer& operator=(const SocketServer& srv); - - /** - * Close the listening socket connection. - */ - void closeListeningSocket(); - - /** - * Get a Byte from the receiving buffer, if any. - * @param[out] byte buffer to write the byte to. - * @returns If a received byte has been succesfully written to the buffer, returns true. Otherwise, false. - */ - bool getByte(uint8_t& byte); + WebotsSerialDrv(); + WebotsSerialDrv(const WebotsSerialDrv& srv); /**< Copy construction of an instance. */ + WebotsSerialDrv& operator=(const WebotsSerialDrv& srv); /**< Assignment of an instance. */ }; -#endif /* SOCKET_SERVER_H_ */ +#endif /* WEBOTS_SERIAL_DRV_H */ /** @} */ diff --git a/lib/Service/Logging.h b/lib/Service/Logging.h index f61edfa9..1dabafcf 100644 --- a/lib/Service/Logging.h +++ b/lib/Service/Logging.h @@ -338,7 +338,7 @@ namespace Logging } } -}; // namespace Logging +}; /* namespace Logging */ /****************************************************************************** * Functions diff --git a/platformio.ini b/platformio.ini index c6d698ab..2f5765b1 100644 --- a/platformio.ini +++ b/platformio.ini @@ -16,11 +16,11 @@ ;default_envs = CalibTarget ;default_envs = CalibSim ;default_envs = ConvoyLeaderTarget -default_envs = ConvoyLeaderSim +;default_envs = ConvoyLeaderSim ; default_envs = ConvoyFollowerSim ; default_envs = ConvoyFollowerTarget ;default_envs = LineFollowerTarget -;default_envs = LineFollowerSim +default_envs = LineFollowerSim ;default_envs = RemoteControlTarget ;default_envs = RemoteControlSim ;default_envs = SensorFusionTarget @@ -113,12 +113,13 @@ lib_ignore = HALTest extra_scripts = ./scripts/webots_launcher.py - pre:./scripts/add_os_specific_build_flags.py pre:./scripts/create_webots_library.py pre:./scripts/copy_sounds.py post:./scripts/copy_webots_shared_libs.py post:./scripts/package.py webots_robot_name = Zumo +webots_robot_serial_rx_channel = 1 +webots_robot_serial_tx_channel = 2 ; ***************************************************************************** ; PC target environment for tests @@ -139,7 +140,6 @@ lib_ignore = HALTarget Webots extra_scripts = - pre:./scripts/add_os_specific_build_flags.py ; ***************************************************************************** ; Calibration application diff --git a/scripts/add_os_specific_build_flags.py b/scripts/add_os_specific_build_flags.py deleted file mode 100644 index 637f30da..00000000 --- a/scripts/add_os_specific_build_flags.py +++ /dev/null @@ -1,55 +0,0 @@ -""" Detect Operative System and add corresponding build flags """ - -# MIT License -# -# Copyright (c) 2023 - 2024 Andreas Merkle -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS -# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. -# - - -################################################################################ -# Imports -################################################################################ -import platform - -Import("env") # pylint: disable=undefined-variable - -################################################################################ -# Variables -################################################################################ - -OS_PLATFORM_TYPE_WIN = "Windows" -OS_PLATFORM_TYPE = platform.system() -WINDOWS_BUILD_FLAGS = ['-lws2_32'] - -################################################################################ -# Classes -################################################################################ - -################################################################################ -# Functions -################################################################################ - -################################################################################ -# Main -################################################################################ - -if OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_WIN: - env.Append(BUILD_FLAGS=WINDOWS_BUILD_FLAGS) # pylint: disable=undefined-variable diff --git a/scripts/webots_launcher.py b/scripts/webots_launcher.py index eee15676..1e6b24a6 100644 --- a/scripts/webots_launcher.py +++ b/scripts/webots_launcher.py @@ -38,9 +38,15 @@ OS_PLATFORM_TYPE_LINUX = "Linux" OS_PLATFORM_TYPE_MACOS = "Darwin" OS_PLATFORM_TYPE = platform.system() -PROGRAM_PATH = "$BUILD_DIR/" -PROGRAM_OPTIONS_SOCKET = '-s' ROBOT_NAME = env.GetProjectOption("webots_robot_name") # pylint: disable=undefined-variable +ROBOT_SERIAL_RX_CHANNEL = env.GetProjectOption("webots_robot_serial_rx_channel") # pylint: disable=undefined-variable +ROBOT_SERIAL_TX_CHANNEL = env.GetProjectOption("webots_robot_serial_tx_channel") # pylint: disable=undefined-variable +PROGRAM_PATH = "$BUILD_DIR/" +PROGRAM_OPTIONS = '' +PROGRAM_OPTIONS_ZUMO_COM_SYSTEM = '-c ' \ + + '--serialRxCh ' + ROBOT_SERIAL_RX_CHANNEL + ' ' \ + + '--serialTxCh ' + ROBOT_SERIAL_TX_CHANNEL + ' ' \ + + '-v' WEBOTS_CONTROLLER_OPTIONS = '--robot-name=' + ROBOT_NAME + ' --stdout-redirect' if OS_PLATFORM_TYPE == OS_PLATFORM_TYPE_WIN: @@ -65,12 +71,13 @@ WEBOTS_LAUNCHER_ACTION = WEBOTS_CONTROLLER + ' '\ + WEBOTS_CONTROLLER_OPTIONS + ' ' \ - + PROGRAM_PATH + PROGRAM_NAME + + PROGRAM_PATH + PROGRAM_NAME + ' ' \ + + PROGRAM_OPTIONS -WEBOTS_LAUNCHER_SOCKET_ACTION = WEBOTS_CONTROLLER + ' ' \ - + WEBOTS_CONTROLLER_OPTIONS + ' ' \ - + PROGRAM_PATH + PROGRAM_NAME + ' ' \ - + PROGRAM_OPTIONS_SOCKET +WEBOTS_LAUNCHER_ZUMO_COM_SYSTEM_ACTION = WEBOTS_CONTROLLER + ' ' \ + + WEBOTS_CONTROLLER_OPTIONS + ' ' \ + + PROGRAM_PATH + PROGRAM_NAME + ' ' \ + + PROGRAM_OPTIONS_ZUMO_COM_SYSTEM ################################################################################ # Classes @@ -91,17 +98,17 @@ actions=[ WEBOTS_LAUNCHER_ACTION ], - title="WebotsLauncher", - description="Launch application with Webots launcher." + title="Launch alone", + description="Launch application with Webots launcher and without ZumoComSystem." ) # pylint: disable=undefined-variable env.AddCustomTarget( - name="webots_launcher_socket", + name="webots_launcher_zumo_com_system", dependencies=PROGRAM_PATH + PROGRAM_NAME, actions=[ - WEBOTS_LAUNCHER_SOCKET_ACTION + WEBOTS_LAUNCHER_ZUMO_COM_SYSTEM_ACTION ], - title="WebotsLauncherSocket", - description="Launch application with Webots launcher and enable socket." + title="Launch with ZumoComSystem", + description="Launch application with Webots launcher and with ZumoComSystem." ) diff --git a/start_platoon.bat b/start_platoon.bat new file mode 100644 index 00000000..41a7c9c8 --- /dev/null +++ b/start_platoon.bat @@ -0,0 +1,33 @@ +@echo off + +set PATH=%PATH%; +set WEBOTS_CONTROLLER="%WEBOTS_HOME%\msys64\mingw64\bin\webots-controller.exe" +set PROGRAM_NAME=program.exe +set CONVOY_LEADER_PROGRAM_PATH=.pio\build\ConvoyLeaderSim +set CONVOY_FOLLOWER_PROGRAM_PATH=.pio\build\ConvoyFollowerSim + +rem Compile Convoy Leader +%USERPROFILE%\.platformio\penv\Scripts\pio.exe run --environment ConvoyLeaderSim + +rem Compile Convoy Follower +%USERPROFILE%\.platformio\penv\Scripts\pio.exe run --environment ConvoyFollowerSim + +rem Start the convoy leader +echo Start convoy leader. +start "Convoy Leader" ""%WEBOTS_CONTROLLER%"" --robot-name=leader --stdout-redirect %CONVOY_LEADER_PROGRAM_PATH%\%PROGRAM_NAME% -c --serialRxCh=1 --serialTxCh=2 -v" + +rem Start the followers +for /L %%i in (1, 1, 2) do ( + call :start_follower %%i +) + +exit /b + +:start_follower +set INSTANCE=%1 +set ROBOT_NAME=follower_%instance% +set /a SERIAL_RX_CHANNEL=(INSTANCE * 2) + 0 +set /a SERIAL_TX_CHANNEL=(INSTANCE * 2) + 1 +echo Start convoy follower %INSTANCE%. +start "Convoy Follower %INSTANCE%" ""%WEBOTS_CONTROLLER%"" --robot-name=%ROBOT_NAME% --stdout-redirect %CONVOY_FOLLOWER_PROGRAM_PATH%\%PROGRAM_NAME% -c --serialRxCh=%SERIAL_RX_CHANNEL% --serialTxCh=%SERIAL_TX_CHANNEL% -v" +exit /b diff --git a/webots/protos/Zumo32U4.proto b/webots/protos/Zumo32U4.proto index b380dee7..913c24f7 100644 --- a/webots/protos/Zumo32U4.proto +++ b/webots/protos/Zumo32U4.proto @@ -5,7 +5,7 @@ EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/project PROTO Zumo32U4 [ field SFVec3f translation 0 0 0 field SFRotation rotation 0 0 1 0 - field SFString name "robot" + field SFString name "Zumo" field SFString contactMaterial "default" # Contact material of the tracks ] { @@ -13,12 +13,16 @@ PROTO Zumo32U4 [ translation IS translation rotation IS rotation children [ - Speaker { + Receiver { + name "serialComRx" + type "serial" } Emitter { name "serialComTx" type "serial" } + Speaker { + } DEF SHIELD Group { children [ Transform { diff --git a/webots/protos/ZumoComSystem.proto b/webots/protos/ZumoComSystem.proto new file mode 100644 index 00000000..58b5bb31 --- /dev/null +++ b/webots/protos/ZumoComSystem.proto @@ -0,0 +1,40 @@ +#VRML_SIM R2023a utf8 + +PROTO ZumoComSystem [ + field SFVec3f translation 0 0 0 + field SFRotation rotation 0 0 1 0 + field SFString name "ZumoComSystem" +] +{ + Robot { + translation IS translation + rotation IS rotation + children [ + DEF BODY Group { + children [ + Shape { + geometry Box { + size 0.06 0.05 0.01 + } + } + ] + } + Receiver { + name "serialComRx" + type "serial" + } + Emitter { + name "serialComTx" + type "serial" + } + ] + name IS name + description "DroidControlShip" + boundingObject USE BODY + physics Physics { + density -1 + mass 0.1 + } + controller "" + } +} \ No newline at end of file diff --git a/webots/worlds/zumo_with_com_system/LineFollowerTrack.wbt b/webots/worlds/zumo_with_com_system/LineFollowerTrack.wbt new file mode 100644 index 00000000..bc65515e --- /dev/null +++ b/webots/worlds/zumo_with_com_system/LineFollowerTrack.wbt @@ -0,0 +1,52 @@ +#VRML_SIM R2023b utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackground.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" +EXTERNPROTO "../../protos/Zumo32U4.proto" +EXTERNPROTO "../../protos/ZumoComSystem.proto" +EXTERNPROTO "../../protos/LineFollowerTrack.proto" +IMPORTABLE EXTERNPROTO "../../protos/Supervisor.proto" + +WorldInfo { + info [ + "Example uses of Track nodes, as caterpillar tracks on a robot, or as a conveyor belt." + ] + title "Track" + basicTimeStep 8 + contactProperties [ + ContactProperties { + material1 "rubber" + material2 "cardboard" + coulombFriction [ + 0.65 + ] + } + ] +} +Viewpoint { + orientation 0.3787583266277594 0.09714544028170935 -0.9203830145339561 2.677946076660944 + position 1.2569587324159737 0.7945022889765716 1.5111964909293643 +} +TexturedBackground { +} +TexturedBackgroundLight { +} +LineFollowerTrack { + contactMaterial "cardboard" +} +DEF ROBOT Zumo32U4 { + translation -0.24713614078815466 -0.04863962992854465 0.013994298332013683 + rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731 + name "Zumo" + contactMaterial "rubber" +} +ZumoComSystem { + translation 0 0 0 + rotation 0 0 1 0 + name "ZumoComSystem" +} +Supervisor { + name "Supervisor" + controller "Supervisor" + supervisor TRUE +} diff --git a/webots/worlds/PlatoonTrack.wbt b/webots/worlds/zumo_with_com_system/PlatoonTrack.wbt similarity index 77% rename from webots/worlds/PlatoonTrack.wbt rename to webots/worlds/zumo_with_com_system/PlatoonTrack.wbt index 894fc68c..a8d2a94b 100644 --- a/webots/worlds/PlatoonTrack.wbt +++ b/webots/worlds/zumo_with_com_system/PlatoonTrack.wbt @@ -2,10 +2,11 @@ EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackground.proto" EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" -EXTERNPROTO "../protos/Zumo32U4.proto" -EXTERNPROTO "../protos/PlatoonTrack.proto" -EXTERNPROTO "../protos/Supervisor.proto" -EXTERNPROTO "../protos/Obstacle.proto" +EXTERNPROTO "../../protos/Zumo32U4.proto" +EXTERNPROTO "../../protos/ZumoComSystem.proto" +EXTERNPROTO "../../protos/PlatoonTrack.proto" +EXTERNPROTO "../../protos/Supervisor.proto" +EXTERNPROTO "../../protos/Obstacle.proto" WorldInfo { info [ @@ -41,18 +42,33 @@ DEF LEADER Zumo32U4 { name "leader" contactMaterial "rubber" } +ZumoComSystem { + translation 0 0 0 + rotation 0 0 1 0 + name "ZumoComSystemLeader" +} DEF FOLLOWER1 Zumo32U4 { translation -3.15 -1.2 0.013994298332013683 rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731 name "follower_1" contactMaterial "rubber" } +ZumoComSystem { + translation 0 0 0 + rotation 0 0 1 0 + name "ZumoComSystemFollower1" +} DEF FOLLOWER2 Zumo32U4 { translation -3.15 -1.35 0.013994298332013683 rotation -1.0564747468923541e-06 8.746699709178704e-07 0.9999999999990595 1.5880805820884731 name "follower_2" contactMaterial "rubber" } +ZumoComSystem { + translation 0 0 0 + rotation 0 0 1 0 + name "ZumoComSystemFollower2" +} Supervisor { name "PlatoonSupervisor" controller "PlatoonSupervisor" diff --git a/webots/worlds/SensorFusionTrack.wbt b/webots/worlds/zumo_with_com_system/SensorFusionTrack.wbt similarity index 78% rename from webots/worlds/SensorFusionTrack.wbt rename to webots/worlds/zumo_with_com_system/SensorFusionTrack.wbt index 468e9c3e..4fe00402 100644 --- a/webots/worlds/SensorFusionTrack.wbt +++ b/webots/worlds/zumo_with_com_system/SensorFusionTrack.wbt @@ -2,9 +2,10 @@ EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackground.proto" EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" -EXTERNPROTO "../protos/Zumo32U4.proto" -EXTERNPROTO "../protos/SensorFusionTrack.proto" -IMPORTABLE EXTERNPROTO "../protos/Supervisor.proto" +EXTERNPROTO "../../protos/Zumo32U4.proto" +EXTERNPROTO "../../protos/ZumoComSystem.proto" +EXTERNPROTO "../../protos/SensorFusionTrack.proto" +IMPORTABLE EXTERNPROTO "../../protos/Supervisor.proto" WorldInfo { info [ @@ -39,6 +40,11 @@ DEF ROBOT Zumo32U4 { name "Zumo" contactMaterial "rubber" } +ZumoComSystem { + translation 0 0 0 + rotation 0 0 1 0 + name "ZumoComSystem" +} Supervisor { name "Supervisor" controller "Supervisor"