From 4004ec12c31b4099c95e1f8890e66710e6e7cb1e Mon Sep 17 00:00:00 2001 From: Raphael Date: Tue, 31 Mar 2020 13:47:01 +0200 Subject: [PATCH] add new comrobot --- .../superviseur-robot/lib/comrobot.cpp | 421 ++++++++++++++++++ .../superviseur-robot/lib/comrobot.h | 198 ++++++++ .../nbproject/private/private.xml | 5 +- .../simulateur/nbproject/private/private.xml | 4 +- 4 files changed, 625 insertions(+), 3 deletions(-) create mode 100644 software/raspberry/superviseur-robot/lib/comrobot.cpp create mode 100644 software/raspberry/superviseur-robot/lib/comrobot.h diff --git a/software/raspberry/superviseur-robot/lib/comrobot.cpp b/software/raspberry/superviseur-robot/lib/comrobot.cpp new file mode 100644 index 0000000..cd1c2f8 --- /dev/null +++ b/software/raspberry/superviseur-robot/lib/comrobot.cpp @@ -0,0 +1,421 @@ +/* + * Copyright (C) 2018 dimercur + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "comrobot.h" + +#include +#include +#include +#include +#include + +#include +#include + +#ifdef __SIMULATION__ +#include +#include +#include +int sock = 0; +#define PORT 6699 +#endif + +#ifdef __FOR_PC__ +#define USART_FILENAME "/dev/ttyUSB0" +#else +#define USART_FILENAME "/dev/ttyS0" +#endif /* __FOR_PC__ */ + +/* + * Constants to be used for communicating with robot. Contains command tag + */ +const char LABEL_ROBOT_PING = 'p'; +const char LABEL_ROBOT_RESET = 'r'; +const char LABEL_ROBOT_START_WITH_WD = 'W'; +const char LABEL_ROBOT_START_WITHOUT_WD = 'u'; +const char LABEL_ROBOT_RELOAD_WD = 'w'; +const char LABEL_ROBOT_MOVE = 'M'; +const char LABEL_ROBOT_TURN = 'T'; +const char LABEL_ROBOT_GET_BATTERY = 'v'; +const char LABEL_ROBOT_GET_STATE = 'b'; +const char LABEL_ROBOT_POWEROFF = 'z'; + +const char LABEL_ROBOT_OK = 'O'; +const char LABEL_ROBOT_ERROR = 'E'; +const char LABEL_ROBOT_UNKNOWN_COMMAND = 'C'; + +const char LABEL_ROBOT_SEPARATOR_CHAR = '='; +const char LABEL_ROBOT_ENDING_CHAR = 0x0D; // carriage return (\\r) + +/** + * Open serial link with robot + * @return File descriptor + * @throw std::runtime_error if it fails + */ +int ComRobot::Open() { + return this->Open(USART_FILENAME); +} + +/** + * Open serial link with robot + * @param usart Filename of usart to open + * @return File descriptor + * @throw std::runtime_error if it fails + */ +int ComRobot::Open(string usart) { + struct termios options; + +#ifdef __SIMULATION__ + + struct sockaddr_in serv_addr; + if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) { + printf("\n Socket creation error \n"); + return -1; + } + struct timeval tv; + tv.tv_sec = 0; + tv.tv_usec = 80000; + setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (const char*) &tv, sizeof tv); + + serv_addr.sin_family = AF_INET; + serv_addr.sin_port = htons(PORT); + + // Convert IPv4 and IPv6 addresses from text to binary form + if (inet_pton(AF_INET, "127.0.0.1", &serv_addr.sin_addr) <= 0) { + printf("\nInvalid address/ Address not supported \n"); + return -1; + } + + if (connect(sock, (struct sockaddr *) &serv_addr, sizeof (serv_addr)) < 0) { + return -1; + } + return 1; +#else + + fd = open(usart.c_str(), O_RDWR | O_NOCTTY /*| O_NDELAY*/); //Open in blocking read/write mode + if (fd == -1) { + cerr << "[" << __PRETTY_FUNCTION__ << "] Unable to open UART (" << usart << "). Ensure it is not in use by another application" << endl << flush; + throw std::runtime_error{"Unable to open UART"}; + exit(EXIT_FAILURE); + } else { + fcntl(fd, F_SETFL, 0); + tcgetattr(fd, &options); + options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + cfsetospeed(&options, B9600); + cfsetispeed(&options, B9600); + options.c_cc[VMIN] = 0; + options.c_cc[VTIME] = 1; /* Timeout of 100 ms per character */ + tcsetattr(fd, TCSANOW, &options); + } + + return fd; +#endif +} + +/** + * Close serial link + * @return Success if above 0, failure if below 0 + */ +int ComRobot::Close() { +#ifdef __SIMULATION__ + return close(sock); +#elif + return close(fd); +#endif +} + +/** + * Send a message to robot + * @param msg Message to send to robot + * @return 1 if success, 0 otherwise + * @attention Message is destroyed (delete) after being sent. You do not need to delete it yourself + * @attention Write is blocking until message is written into buffer (linux side) + * @warning Write is not thread save : check that multiple tasks can't access this method simultaneously + */ +Message *ComRobot::Write(Message* msg) { + Message *msgAnswer; + string s; + + if (this->fd != -1) { + + Write_Pre(); + + s = MessageToString(msg); +#ifdef __SIMULATION__ + + char buffer[1024] = {0}; + cout << "[" << __PRETTY_FUNCTION__ << "] Send command: " << s << endl << flush; + send(sock, s.c_str(), s.length(), MSG_NOSIGNAL); + + int valread = read(sock, buffer, 1024); + + if (valread == 0) { + cout << "The communication is out of order" << endl; + msgAnswer = new Message(MESSAGE_ANSWER_COM_ERROR); + } else if (valread < 0) { + cout << "Timeout" << endl; + msgAnswer = new Message(MESSAGE_ANSWER_ROBOT_TIMEOUT); + } else { + string s(&buffer[0], valread); + msgAnswer = StringToMessage(s); + cout << "Response: " << buffer << ", id: " << msgAnswer->GetID() << endl; + } + +#else + AddChecksum(s); + + //cout << "[" <<__PRETTY_FUNCTION__<<"] Send command: "<fd, s.c_str(), s.length()); //Filestream, bytes to write, number of bytes to write + + if (count < 0) { + cerr << "[" << __PRETTY_FUNCTION__ << "] UART TX error (" << to_string(count) << ")" << endl << flush; + msgAnswer = new Message(MESSAGE_ANSWER_COM_ERROR); + } else { /* write successfull, read answer from robot */ + + try { + s = Read(); + //cout << "Answer = "<fd, (void*) &receivedChar, 1); //Filestream, buffer to store in, number of bytes to read (max) + if (rxLength == 0) { // timeout + // try again + rxLength = read(this->fd, (void*) &receivedChar, 1); //Filestream, buffer to store in, number of bytes to read (max) + if (rxLength == 0) { // re-timeout: it sucks ! + throw std::runtime_error{"ComRobot::Read: Timeout when reading from com port"}; + } + } else if (rxLength < 0) { // big pb ! + throw std::runtime_error{"ComRobot::Read: Unknown problem when reading from com port"}; + } else { // everything ok + if ((receivedChar != '\r') && (receivedChar != '\n')) s += receivedChar; + } + } while ((receivedChar != '\r') && (receivedChar != '\n')); + + return s; +} + +Message *ComRobot::SendCommand(Message* msg, MessageID answerID, int maxRetries) { + int counter = maxRetries; + Message *msgSend; + Message *msgRcv; + Message *msgTmp; + + do { + msgSend = msg->Copy(); + cout << "S => " << msgSend->ToString() << endl << flush; + msgTmp = Write(msgSend); + cout << "R <= " << msgTmp->ToString() << endl << flush; + + if (msgTmp->CompareID(answerID)) counter = 0; + else counter--; + + if (counter == 0) msgRcv = msgTmp->Copy(); + + delete(msgTmp); + } while (counter); + + delete (msg); + + return msgRcv; +} + +/** + * Convert an array of char to its message representation (when receiving data from stm32) + * @param bytes Array of char + * @return Message corresponding to received array of char + */ +Message* ComRobot::StringToMessage(string s) { + Message *msg; + + switch (s[0]) { + case LABEL_ROBOT_OK: + msg = new Message(MESSAGE_ANSWER_ACK); + break; + case LABEL_ROBOT_ERROR: + msg = new Message(MESSAGE_ANSWER_ROBOT_ERROR); + break; + case LABEL_ROBOT_UNKNOWN_COMMAND: + msg = new Message(MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND); + break; + case '0': + msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_EMPTY); + break; + case '1': + msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_LOW); + break; + case '2': + msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_FULL); + break; + default: + msg = new Message(MESSAGE_ANSWER_ROBOT_ERROR); + cerr << "[" << __PRETTY_FUNCTION__ << "] Unknown message received from robot (" << s << ")" << endl << flush; + } + + return msg; +} + +/** + * Convert a message to its array of char representation (for sending command to stm32) + * @param msg Message to be sent to robot + * @param buffer Array of char, image of message to send + */ +string ComRobot::MessageToString(Message *msg) { + string s; + + float val_f; + int val_i; + unsigned char *b; + + switch (msg->GetID()) { + case MESSAGE_ROBOT_PING: + s += LABEL_ROBOT_PING; + break; + case MESSAGE_ROBOT_RESET: + s += LABEL_ROBOT_RESET; + break; + case MESSAGE_ROBOT_POWEROFF: + s += LABEL_ROBOT_POWEROFF; + break; + case MESSAGE_ROBOT_START_WITHOUT_WD: + s += LABEL_ROBOT_START_WITHOUT_WD; + break; + case MESSAGE_ROBOT_START_WITH_WD: + s += LABEL_ROBOT_START_WITH_WD; + break; + case MESSAGE_ROBOT_RELOAD_WD: + s += LABEL_ROBOT_RELOAD_WD; + break; + case MESSAGE_ROBOT_BATTERY_GET: + s += LABEL_ROBOT_GET_BATTERY; + break; + case MESSAGE_ROBOT_STATE_GET: + s += LABEL_ROBOT_GET_STATE; + break; + case MESSAGE_ROBOT_GO_FORWARD: + s += LABEL_ROBOT_MOVE; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(500000)); + break; + case MESSAGE_ROBOT_GO_BACKWARD: + s += LABEL_ROBOT_MOVE; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(-500000)); + break; + case MESSAGE_ROBOT_GO_LEFT: + s += LABEL_ROBOT_TURN; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(-500000)); + break; + case MESSAGE_ROBOT_GO_RIGHT: + s += LABEL_ROBOT_TURN; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(500000)); + break; + case MESSAGE_ROBOT_STOP: + s += LABEL_ROBOT_MOVE; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(0)); + break; + case MESSAGE_ROBOT_MOVE: + s += LABEL_ROBOT_MOVE; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(((MessageInt*) msg)->GetValue())); + break; + case MESSAGE_ROBOT_TURN: + s += LABEL_ROBOT_TURN; + s += LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(((MessageInt*) msg)->GetValue())); + break; + default: + cerr << "[" << __PRETTY_FUNCTION__ << "] Invalid message for robot (" << msg->ToString() << ")" << endl << flush; + throw std::runtime_error{"Invalid message"}; + } + + return s; +} + +/** + * Add a checksum and carriage return to a command string + * @param[in,out] s String containing command for robot, without ending char (carriage return) + */ +void ComRobot::AddChecksum(string &s) { + unsigned char checksum = 0; + + for (string::iterator it = s.begin(); it != s.end(); ++it) { + checksum ^= (unsigned char) *it; + } + + s += (char) checksum; // Add calculated checksum + s += (char) LABEL_ROBOT_ENDING_CHAR; +} + +/** + * Verify if checksum of an incoming answer from robot is valid, + * then remove checksum from incoming answer (if checksum is ok) + * @param[in,out] s String containing incoming answer from robot + * @return true is checksum is valid, false otherwise. + */ +bool ComRobot::VerifyChecksum(string &s) { + unsigned char checksum = 0; + + for (string::iterator it = s.begin(); it != s.end(); ++it) { + checksum ^= (unsigned char) *it; + } + + if (checksum == 0) { // checksum is ok, remove last char of string (checksum) + s.pop_back(); // remove last char + return true; + } else return false; +} diff --git a/software/raspberry/superviseur-robot/lib/comrobot.h b/software/raspberry/superviseur-robot/lib/comrobot.h new file mode 100644 index 0000000..d6bdd11 --- /dev/null +++ b/software/raspberry/superviseur-robot/lib/comrobot.h @@ -0,0 +1,198 @@ +/* + * Copyright (C) 2018 dimercur + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#ifndef __COMROBOT_H__ +#define __COMROBOT_H__ + +#include "messages.h" +#include + +using namespace std; + +/** + * Class used for communicating with robot over serial + * + * @brief Communication class with robot + * + */ +class ComRobot { +public: + + /** + * Constructor + */ + ComRobot() { + } + + /** + * Destructor + */ + virtual ~ComRobot() { + } + + /** + * Open serial link with robot + * @return File descriptor + * @throw std::runtime_error if it fails + */ + int Open(); + + /** + * Open serial link with robot + * @param usart Filename of usart to open + * @return File descriptor + * @throw std::runtime_error if it fails + */ + int Open(string usart); + + /** + * Close serial link + * @return Success if above 0, failure if below 0 + */ + int Close(); + + /** + * Send a message to robot + * @param msg Message to send to robot + * @return A message containing either an answer (Ack/Nak/Timeout/Error) or a value (battery level, robot state) depending of the command + * @attention Input message is destroyed (delete) after being sent. You do not need to delete it yourself + * @attention Write produce an answer message. You have to dispose it (delete) when you have finished using it + * @attention Write is blocking until message is written into buffer (linux side) + * @warning Write is not thread save : check that multiple tasks can't access this method simultaneously + */ + Message *Write(Message* msg); + + /** + * Function called at beginning of Write method + * Use it to do some synchronization (call of mutex, for example) + */ + virtual void Write_Pre() { + } + + /** + * Function called at end of Write method + * Use it to do some synchronization (call of mutex, for example) + */ + virtual void Write_Post() { + } + + Message *SendCommand(Message* msg, MessageID answerID, int maxRetries); + + static Message *Ping() { + return new Message(MESSAGE_ROBOT_PING); + } + + static Message *Reset() { + return new Message(MESSAGE_ROBOT_RESET); + } + + static Message *PowerOff() { + return new Message(MESSAGE_ROBOT_POWEROFF); + } + + static Message *StartWithWD() { + return new Message(MESSAGE_ROBOT_START_WITH_WD); + } + + static Message *StartWithoutWD() { + return new Message(MESSAGE_ROBOT_START_WITHOUT_WD); + } + + static Message *ReloadWD() { + return new Message(MESSAGE_ROBOT_RELOAD_WD); + } + + static Message *Move(int length) { + return new MessageInt(MESSAGE_ROBOT_MOVE, length); + } + + static Message *Turn(int angle) { + return new MessageInt(MESSAGE_ROBOT_TURN, angle); + } + + static Message *Stop() { + return new Message(MESSAGE_ROBOT_STOP); + } + + static Message *GoForward() { + return new Message(MESSAGE_ROBOT_GO_FORWARD); + } + + static Message *GoBackward() { + return new Message(MESSAGE_ROBOT_GO_BACKWARD); + } + + static Message *GoLeft() { + return new Message(MESSAGE_ROBOT_GO_LEFT); + } + + static Message *GoRight() { + return new Message(MESSAGE_ROBOT_GO_RIGHT); + } + + static Message *GetBattery() { + return new Message(MESSAGE_ROBOT_BATTERY_GET); + } + + static Message *GetState() { + return new Message(MESSAGE_ROBOT_STATE_GET); + } + +protected: + /** + * Serial link file descriptor + */ + int fd; + + /** + * Get an answer from robot + * @return String containing answer from robot + * @attention Read method is blocking until a message is received (timeout of 500 ms) + * @warning Read is not thread safe : Do not call it in multiple tasks simultaneously + */ + string Read(); + + /** + * Convert a string to its message representation (when receiving data from robot) + * @param s String from robot containing answer + * @return Message corresponding to received array of char + */ + Message* StringToMessage(string s); + + /** + * Convert a message to its string representation (for sending command to robot) + * @param msg Message to be sent to robot + * @return String containing command to robot + */ + string MessageToString(Message *msg); + + /** + * Add a checksum and carriage return to a command string + * @param[in,out] s String containing command for robot, without ending char (carriage return) + */ + void AddChecksum(string &s); + + /** + * Verify if checksum of an incoming answer from robot is valid, + * then remove checksum from incoming answer (if checksum is ok) + * @param[in,out] s String containing incoming answer from robot + * @return true is checksum is valid, false otherwise. + */ + bool VerifyChecksum(string &s); +}; + +#endif /* __COMROBOT_H__ */ diff --git a/software/raspberry/superviseur-robot/nbproject/private/private.xml b/software/raspberry/superviseur-robot/nbproject/private/private.xml index 80a7712..e6d6c63 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/private.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/private.xml @@ -8,11 +8,12 @@ file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/tasks.cpp - file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/comrobot.cpp - file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/commonitor.cpp file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/tasks.h file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/main.cpp file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/comrobot.h + file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/commonitor.cpp + file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/comrobot.cpp + file:/home/raphael/Documents/real_time/software/raspberry/superviseur-robot/lib/messages.h diff --git a/software/simulateur/nbproject/private/private.xml b/software/simulateur/nbproject/private/private.xml index aef7ea3..9b42543 100644 --- a/software/simulateur/nbproject/private/private.xml +++ b/software/simulateur/nbproject/private/private.xml @@ -6,6 +6,8 @@ - + + file:/home/raphael/Documents/real_time/software/simulateur/main.cpp +