diff --git a/software/raspberry/superviseur-robot/dist/Debug__Pthread_/GNU-Linux/superviseur-robot b/software/raspberry/superviseur-robot/dist/Debug__Pthread_/GNU-Linux/superviseur-robot new file mode 100755 index 0000000..ac66d60 Binary files /dev/null and b/software/raspberry/superviseur-robot/dist/Debug__Pthread_/GNU-Linux/superviseur-robot differ diff --git a/software/raspberry/superviseur-robot/lib/camera.cpp b/software/raspberry/superviseur-robot/lib/camera.cpp index f14155a..4203b61 100644 --- a/software/raspberry/superviseur-robot/lib/camera.cpp +++ b/software/raspberry/superviseur-robot/lib/camera.cpp @@ -1,4 +1,4 @@ -/* + /* * Copyright (C) 2018 dimercur * * This program is free software: you can redistribute it and/or modify diff --git a/software/raspberry/superviseur-robot/lib/commonitor.cpp b/software/raspberry/superviseur-robot/lib/commonitor.cpp index 9bb0d96..fa3aa36 100644 --- a/software/raspberry/superviseur-robot/lib/commonitor.cpp +++ b/software/raspberry/superviseur-robot/lib/commonitor.cpp @@ -33,25 +33,25 @@ */ const string LABEL_MONITOR_ANSWER_ACK = "AACK"; const string LABEL_MONITOR_ANSWER_NACK = "ANAK"; -const string LABEL_MONITOR_ANSWER_LOST_DMB= "ATIM"; -const string LABEL_MONITOR_ANSWER_TIMEOUT= "ATIM"; -const string LABEL_MONITOR_ANSWER_CMD_REJECTED= "ACRJ"; +const string LABEL_MONITOR_ANSWER_COM_ERROR = "ACER"; +const string LABEL_MONITOR_ANSWER_TIMEOUT = "ATIM"; +const string LABEL_MONITOR_ANSWER_CMD_REJECTED = "ACRJ"; const string LABEL_MONITOR_MESSAGE = "MSSG"; -const string LABEL_MONITOR_CAMERA_OPEN= "COPN"; -const string LABEL_MONITOR_CAMERA_CLOSE= "CCLS"; +const string LABEL_MONITOR_CAMERA_OPEN = "COPN"; +const string LABEL_MONITOR_CAMERA_CLOSE = "CCLS"; const string LABEL_MONITOR_CAMERA_IMAGE = "CIMG"; const string LABEL_MONITOR_CAMERA_ARENA_ASK = "CASA"; const string LABEL_MONITOR_CAMERA_ARENA_INFIRME = "CAIN"; const string LABEL_MONITOR_CAMERA_ARENA_CONFIRM = "CACO"; -const string LABEL_MONITOR_CAMERA_POSITION_COMPUTE= "CPCO"; -const string LABEL_MONITOR_CAMERA_POSITION_STOP= "CPST"; +const string LABEL_MONITOR_CAMERA_POSITION_COMPUTE = "CPCO"; +const string LABEL_MONITOR_CAMERA_POSITION_STOP = "CPST"; const string LABEL_MONITOR_CAMERA_POSITION = "CPOS"; const string LABEL_MONITOR_ROBOT_COM_OPEN = "ROPN"; const string LABEL_MONITOR_ROBOT_COM_CLOSE = "RCLS"; const string LABEL_MONITOR_ROBOT_PING = "RPIN"; const string LABEL_MONITOR_ROBOT_RESET = "RRST"; -const string LABEL_MONITOR_ROBOT_START_WITHOUT_WD= "RSOW"; -const string LABEL_MONITOR_ROBOT_START_WITH_WD= "RSWW"; +const string LABEL_MONITOR_ROBOT_START_WITHOUT_WD = "RSOW"; +const string LABEL_MONITOR_ROBOT_START_WITH_WD = "RSWW"; const string LABEL_MONITOR_ROBOT_RELOAD_WD = "RLDW"; const string LABEL_MONITOR_ROBOT_MOVE = "RMOV"; const string LABEL_MONITOR_ROBOT_TURN = "RTRN"; @@ -80,7 +80,7 @@ int ComMonitor::Open(int port) { socketFD = socket(AF_INET, SOCK_STREAM, 0); if (socketFD < 0) { - throw std::runtime_error{"ComMonitor::Open : Can not create socket"}; + throw std::runtime_error{"Can not create socket"}; } server.sin_addr.s_addr = INADDR_ANY; @@ -88,7 +88,8 @@ int ComMonitor::Open(int port) { server.sin_port = htons(port); if (bind(socketFD, (struct sockaddr *) &server, sizeof (server)) < 0) { - throw std::runtime_error{"ComMonitor::Open : Can not bind socket on port " + std::to_string(port)}; + cerr<<"["<<__PRETTY_FUNCTION__<<"] Can not bind socket ("< 0) { if (data != '\n') { - s+=data; + s += data; } else endReception = true; } } - if (length<=0) msg = new Message(MESSAGE_MONITOR_LOST); + if (length <= 0) msg = new Message(MESSAGE_MONITOR_LOST); else { - msg=StringToMessage(s); + msg = StringToMessage(s); } } // Call user method after read Read_Post(); - + return msg; } @@ -197,62 +196,56 @@ string ComMonitor::MessageToString(Message &msg) { string str; Message *localMsg = &msg; Position pos; - + id = msg.GetID(); switch (id) { - case MESSAGE_ANSWER: - switch (((MessageAnswer*)localMsg)->GetAnswer()) { - case ANSWER_ACK: - str.append(LABEL_MONITOR_ANSWER_ACK); - break; - case ANSWER_NACK: - str.append(LABEL_MONITOR_ANSWER_NACK); - break; - case ANSWER_LOST_ROBOT: - str.append(LABEL_MONITOR_ANSWER_LOST_DMB); - break; - case ANSWER_ROBOT_TIMEOUT: - str.append(LABEL_MONITOR_ANSWER_TIMEOUT); - break; - case ANSWER_ROBOT_UNKNOWN_COMMAND: - str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED); - break; - case ANSWER_ROBOT_ERROR: - str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED); - break; - default: - str.append(LABEL_MONITOR_ANSWER_NACK); - }; - + case MESSAGE_ANSWER_ACK : + str.append(LABEL_MONITOR_ANSWER_ACK); break; - case MESSAGE_POSITION: - pos = ((MessagePosition*)&msg)->GetPosition(); - + case MESSAGE_ANSWER_NACK: + str.append(LABEL_MONITOR_ANSWER_NACK); + break; + case MESSAGE_ANSWER_ROBOT_TIMEOUT: + str.append(LABEL_MONITOR_ANSWER_TIMEOUT); + break; + case MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND: + str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED); + break; + case MESSAGE_ANSWER_ROBOT_ERROR: + str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED); + break; + case MESSAGE_ANSWER_COM_ERROR: + str.append(LABEL_MONITOR_ANSWER_COM_ERROR); + break; + case MESSAGE_CAM_POSITION: + pos = ((MessagePosition*) & msg)->GetPosition(); + str.append(LABEL_MONITOR_CAMERA_POSITION + LABEL_SEPARATOR_CHAR + to_string(pos.robotId) + ";" + to_string(pos.angle) + ";" + to_string(pos.center.x) + ";" + to_string(pos.center.y) + ";" + to_string(pos.direction.x) + ";" + to_string(pos.direction.y)); break; - case MESSAGE_IMAGE: - str.append(LABEL_MONITOR_CAMERA_IMAGE + LABEL_SEPARATOR_CHAR + ((MessageImg*) &msg)->GetImage()->ToBase64()); + case MESSAGE_CAM_IMAGE: + str.append(LABEL_MONITOR_CAMERA_IMAGE + LABEL_SEPARATOR_CHAR + ((MessageImg*) & msg)->GetImage()->ToBase64()); break; case MESSAGE_ROBOT_BATTERY_LEVEL: - str.append(LABEL_MONITOR_ROBOT_BATTERY_LEVEL + LABEL_SEPARATOR_CHAR + to_string(((MessageBattery*) &msg)->GetLevel())); + str.append(LABEL_MONITOR_ROBOT_BATTERY_LEVEL + LABEL_SEPARATOR_CHAR + to_string(((MessageBattery*) & msg)->GetLevel())); break; - case MESSAGE_ROBOT_CURRENT_STATE: - str.append(LABEL_MONITOR_ROBOT_CURRENT_STATE + LABEL_SEPARATOR_CHAR + to_string(((MessageState*) &msg)->GetState())); + case MESSAGE_ROBOT_STATE_BUSY: + str.append(LABEL_MONITOR_ROBOT_CURRENT_STATE + LABEL_SEPARATOR_CHAR + "1"); + break; + case MESSAGE_ROBOT_STATE_NOT_BUSY: + str.append(LABEL_MONITOR_ROBOT_CURRENT_STATE + LABEL_SEPARATOR_CHAR + "0"); break; case MESSAGE_LOG: - str.append(LABEL_MONITOR_MESSAGE + LABEL_SEPARATOR_CHAR + ((MessageString*) &msg)->GetString()); + str.append(LABEL_MONITOR_MESSAGE + LABEL_SEPARATOR_CHAR + ((MessageString*) & msg)->GetString()); break; case MESSAGE_EMPTY: str.append(""); //empty string break; default: - throw std::runtime_error - { - "ComMonitor::MessageToString (from ComMonitor::Write): Invalid message to send (" + msg.ToString() - }; + cerr<<"["<<__PRETTY_FUNCTION__<<"] (from ComMonitor::Write): Invalid message to send ("<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; + + fd = open(usart.c_str(), O_RDWR | O_NOCTTY /*| O_NDELAY*/); //Open in blocking read/write mode if (fd == -1) { - //ERROR - CAN'T OPEN SERIAL PORT - throw std::runtime_error{"Error - Unable to open UART " + string(USART_FILENAME) + ". Ensure it is not in use by another application"}; + cerr<<"["<<__PRETTY_FUNCTION__<<"] Unable to open UART ("<fd, (void*) &receivedChar, 1); //Filestream, buffer to store in, number of bytes to read (max) - //printf ("W=%02X ", receivedChar); - - if (rxLength <= -1) { - this->lostCom = true; - printf("Warning: communication lost in ComStm32::Read\n"); - msg = new Message(); - - return msg; - } else if (rxLength == 0) { - // nothing to do - } else if (receivedChar == '<') { // start of frame received - i = 0; - - do { - rxLength = read(this->fd, (void*) &rxBuffer[i], 6 - i); //Filestream, buffer to store in, number of bytes to read (max) - - if (rxLength >= 0) - i = i + rxLength; - else { - printf("Error while reading (%i)", rxLength); - - return NULL; - } - } while (i < 6); - - if (rxBuffer[5] == '\n') { - messageComplete = true; - } - } - } - - /* Treatment of received message */ - msg = CharToMessage(rxBuffer); - - /* Call Post method for read */ - Read_Post(); - - return msg; -} - -/** - * 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::CharToMessage(unsigned char *bytes) { - Message *msg = __null; - MessageFloat *msgf; - MessageBool *msgb; - - switch (bytes[0]) { - case LABEL_ANGLE_POSITION: - msgf = new MessageFloat(); - msgf->SetID(MESSAGE_ANGLE_POSITION); - msgf->SetValue(CharToFloat(&bytes[1])); - msg = (Message*) msgf; - - break; - case LABEL_ANGULAR_SPEED: - msgf = new MessageFloat(); - msgf->SetID(MESSAGE_ANGULAR_SPEED); - msgf->SetValue(CharToFloat(&bytes[1])); - msg = (Message*) msgf; - - break; - case LABEL_BATTERY_LEVEL: - msgf = new MessageFloat(); - msgf->SetID(MESSAGE_BATTERY); - msgf->SetValue(CharToFloat(&bytes[1])); - msg = (Message*) msgf; - - break; - case LABEL_BETA_ANGLE: - msgf = new MessageFloat(); - msgf->SetID(MESSAGE_BETA); - msgf->SetValue(CharToFloat(&bytes[1])); - msg = (Message*) msgf; - - break; - case LABEL_USER_PRESENCE: - msgb = new MessageBool(); - msgb->SetID(MESSAGE_USER_PRESENCE); - msgb->SetState(CharToBool(&bytes[1])); - msg = (Message*) msgb; - - break; - default: - printf("Unknown message received from robot (%i)\n", bytes[0]); - fflush(stdout); - msg = new Message(); - } - - if (msg == NULL) { - printf("Message is null (%02X)\n", bytes[0]); - fflush(stdout); - msg = new Message(); - } - - return msg; -} - -/** - * Convert an array of char to float - * @param bytes Array of char, containing a binary image of a float - * @return Float value - */ -float ComRobot::CharToFloat(unsigned char *bytes) { - unsigned long value; - - union { - unsigned char buffer[4]; - float f; - } convert; - - convert.buffer[0] = bytes[0]; - convert.buffer[1] = bytes[1]; - convert.buffer[2] = bytes[2]; - convert.buffer[3] = bytes[3]; - - //value = (bytes[3] << 24) | (bytes[2] << 16) | (bytes[1] << 8) | (bytes[0]); - - return convert.f; -} - -/** - * Convert an array of char to integer - * @param bytes Array of char, containing a binary image of an integer - * @return Integer value - */ -unsigned int ComRobot::CharToInt(unsigned char *bytes) { - unsigned long value; - - value = (bytes[3] << 24) | (bytes[2] << 16) | (bytes[1] << 8) | (bytes[0]); - - return (unsigned int) value; -} - -/** - * Convert an array of char to boolean - * @param bytes Array of char, containing a binary image of a boolean - * @return Boolean value - */ -bool ComRobot::CharToBool(unsigned char *bytes) { - unsigned long value; - - value = (bytes[3] << 24) | (bytes[2] << 16) | (bytes[1] << 8) | (bytes[0]); - - if (value == 0) return false; - - else return true; -} - /** * Send a message to robot * @param msg Message to send to robot @@ -266,29 +107,142 @@ bool ComRobot::CharToBool(unsigned char *bytes) { * @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 */ -int ComRobot::Write(Message* msg) { - unsigned char buffer[7]; - int ret_val = 0; - - MessageToChar(msg, buffer); - - Write_Pre(); +Message *ComRobot::Write(Message* msg) { + Message *msgAnswer; + string s; if (this->fd != -1) { - int count = write(this->fd, &buffer[0], 7); //Filestream, bytes to write, number of bytes to write + + Write_Pre(); + + s=MessageToString(msg); + 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) { - printf("Warning: UART TX error in ComStm32::Write\n"); - } else { - ret_val = 1; + 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 <<")"<GetID()) { - case MESSAGE_TORQUE: - buffer[1] = LABEL_TORQUE; - val_f = ((MessageFloat*) msg)->GetValue(); - b = (unsigned char *) &val_f; - + case MESSAGE_ROBOT_PING: + s+=LABEL_ROBOT_PING; break; - case MESSAGE_EMERGENCY_STOP: - buffer[1] = LABEL_EMERGENCY_STOP; - if (((MessageBool*) msg)->GetState()) - val_i = 1; - else - val_i = 0; - b = (unsigned char *) &val_i; - + 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(90)); + break; + case MESSAGE_ROBOT_GO_RIGHT: + s+=LABEL_ROBOT_TURN; + s+=LABEL_ROBOT_SEPARATOR_CHAR; + s.append(to_string(-90)); + 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: - printf("Invalid message to send"); - val_i = 0; - b = (unsigned char *) &val_i; + cerr<<"["<<__PRETTY_FUNCTION__<<"] Invalid message for robot ("<ToString()<<")"< using namespace std; @@ -30,114 +31,168 @@ using namespace std; */ class ComRobot { public: + /** * Constructor */ - ComRobot() {} - + ComRobot() { + } + /** * Destructor */ - virtual ~ComRobot() {} - + 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(); - - /** - * Get a message from robot - * @return Message currently received - * @attention A message object is created (new) when receiving data from robot. You MUST remember to destroy is (delete) after use - * @attention Read method is blocking until a message is received - * @warning Read is not thread safe : Do not call it in multiple tasks simultaneously - */ - Message* Read(); - + /** * 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 + * @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 */ - int Write(Message* msg); - - /** - * Function called at beginning of Read method - * Use it to do some synchronization (call of mutex, for example) - */ - virtual void Read_Pre() {} - - /** - * Function called at end of Read method - * Use it to do some synchronization (call of mutex, for example) - */ - virtual void Read_Post() {} - + 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() {} - + 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() {} + virtual void Write_Post() { + } - static Message *Ping(); + 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; - + /** - * Convert an array of char to float - * @param bytes Array of char, containing a binary image of a float - * @return Float value + * 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 */ - float CharToFloat(unsigned char *bytes); - + string Read(); + /** - * Convert an array of char to boolean - * @param bytes Array of char, containing a binary image of a boolean - * @return Boolean value - */ - bool CharToBool(unsigned char *bytes); - - /** - * Convert an array of char to integer - * @param bytes Array of char, containing a binary image of an integer - * @return Integer value - */ - unsigned int CharToInt(unsigned char *bytes); - - /** - * Convert an array of char to its message representation (when receiving data from stm32) - * @param bytes Array of char + * 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* CharToMessage(unsigned char *bytes); - + Message* StringToMessage(string s); + /** - * Convert a message to its array of char representation (for sending command to stm32) + * Convert a message to its string representation (for sending command to robot) * @param msg Message to be sent to robot - * @param buffer Array of char, image of message to send + * @return String containing command to robot */ - void MessageToChar(Message *msg, unsigned char *buffer); + 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/lib/img.cpp b/software/raspberry/superviseur-robot/lib/img.cpp index 4b7ae3a..ffbd655 100644 --- a/software/raspberry/superviseur-robot/lib/img.cpp +++ b/software/raspberry/superviseur-robot/lib/img.cpp @@ -27,9 +27,13 @@ Img::Img(ImageMat imgMatrice) { } string Img::ToString() { - return "Image size: "+this->img.cols+"x"this->img.rows+" (dim="+this->img.dims+")"; + return "Image size: "+to_string(this->img.cols)+"x"+to_string(this->img.rows)+" (dim="+to_string(this->img.dims)+")"; } +string Img::ToBase64() { + return ""; +} + Img* Img::Copy() { return new Img(this->img); } @@ -100,13 +104,13 @@ Jpg Img::toJpg() { return imgJpg; } -string Img::ToBase64() { - string imgBase64; - Jpg imgJpg = toJpg(); - - /* faire la convertion Jpg vers base 64 */ - return imgBase64; -} +//string Img::ToBase64() { +// string imgBase64; +// Jpg imgJpg = toJpg(); +// +// /* faire la convertion Jpg vers base 64 */ +// return imgBase64; +//} std::list Img::search_robot(Arene monArene) { diff --git a/software/raspberry/superviseur-robot/lib/img.h b/software/raspberry/superviseur-robot/lib/img.h index 4e98ea0..dc0efe8 100644 --- a/software/raspberry/superviseur-robot/lib/img.h +++ b/software/raspberry/superviseur-robot/lib/img.h @@ -49,7 +49,7 @@ typedef struct { class Arene { public: - Arene(); + Arene() {} cv::Rect arene; bool empty(); diff --git a/software/raspberry/superviseur-robot/lib/messages.cpp b/software/raspberry/superviseur-robot/lib/messages.cpp index 8957c62..904bc2f 100644 --- a/software/raspberry/superviseur-robot/lib/messages.cpp +++ b/software/raspberry/superviseur-robot/lib/messages.cpp @@ -27,7 +27,12 @@ const string MESSAGE_ID_STRING[] = { "Empty", "Log", - "Answer", + "Answer [Acknowledge]", + "Answer [Not Acknowledge]", + "Answer [Command timeout]", + "Answer [Command unknown]", + "Answer [Command error]", + "Answer [Communication error]", "Monitor connection lost", "Open serial com", "Close serial com", @@ -42,8 +47,8 @@ const string MESSAGE_ID_STRING[] = { "Image", "Robot ping", "Robot reset", - "Robot start with wtachdog", - "Robot start without wtachdog", + "Robot start with watchdog", + "Robot start without watchdog", "Robot reload watchdog", "Robot move", "Robot turn", @@ -56,7 +61,9 @@ const string MESSAGE_ID_STRING[] = { "Robot get battery", "Robot battery level", "Robot get state", - "Robot current state" + "Robot current state", + "Robot state [Not busy]", + "Robot state [Busy]" }; /* @@ -108,7 +115,7 @@ void Message::SetID(MessageID id) { */ string Message::ToString() { if (CheckID(this->messageID)) - return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\""; + return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\""; else return "Invalid message"; } @@ -118,7 +125,7 @@ string Message::ToString() { * @return A message, copy of current */ Message* Message::Copy() { - Message *msg = new Message(); + Message *msg = new Message(this->messageID); return msg; } @@ -128,30 +135,12 @@ Message* Message::Copy() { * @return Current message ID */ bool Message::CheckID(MessageID id) { - if ((id != MESSAGE_EMPTY) && - (id != MESSAGE_MONITOR_LOST) && - (id != MESSAGE_ARENA_CONFIRM) && - (id != MESSAGE_ARENA_INFIRM) && - (id != MESSAGE_ASK_ARENA) && - (id != MESSAGE_CAM_CLOSE) && - (id != MESSAGE_CAM_OPEN) && - (id != MESSAGE_CLOSE_COM) && - (id != MESSAGE_COMPUTE_POSITION) && - (id != MESSAGE_OPEN_COM) && - (id != MESSAGE_ROBOT_GET_BATTERY) && - (id != MESSAGE_ROBOT_GET_STATE) && - (id != MESSAGE_ROBOT_GO_BACK) && - (id != MESSAGE_ROBOT_GO_FORWARD) && - (id != MESSAGE_ROBOT_GO_LEFT) && - (id != MESSAGE_ROBOT_GO_RIGHT) && - (id != MESSAGE_ROBOT_PING) && - (id != MESSAGE_ROBOT_POWEROFF) && - (id != MESSAGE_ROBOT_RELOAD_WD) && - (id != MESSAGE_ROBOT_RESET) && - (id != MESSAGE_ROBOT_START_WITHOUT_WD) && - (id != MESSAGE_ROBOT_START_WITH_WD) && - (id != MESSAGE_ROBOT_STOP) && - (id != MESSAGE_STOP_COMPUTE_POSITION)) { + if ((id == MESSAGE_CAM_IMAGE) || + (id == MESSAGE_CAM_POSITION) || + (id == MESSAGE_ROBOT_MOVE) || + (id == MESSAGE_ROBOT_TURN) || + (id == MESSAGE_LOG) || + (id == MESSAGE_ROBOT_BATTERY_LEVEL)) { return false; } else return true; } @@ -197,7 +186,7 @@ void MessageInt::SetID(MessageID id) { */ string MessageInt::ToString() { if (CheckID(this->messageID)) - return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nValue: " + to_string(this->value); + return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nValue: " + to_string(this->value); else return "Invalid message"; } @@ -263,7 +252,7 @@ void MessageString::SetID(MessageID id) { */ string MessageString::ToString() { if (CheckID(this->messageID)) - return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nString: \"" + this->s + "\""; + return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nString: \"" + this->s + "\""; else return "Invalid message"; } @@ -342,7 +331,7 @@ void MessageImg::SetImage(Img* image) { */ string MessageImg::ToString() { if (CheckID(this->messageID)) - return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\n" + this->image->ToString(); + return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\n" + this->image->ToString(); else return "Invalid message"; } @@ -362,91 +351,7 @@ Message* MessageImg::Copy() { * @return true, if message ID is acceptable, false otherwise */ bool MessageImg::CheckID(MessageID id) { - if (id != MESSAGE_IMAGE) { - return false; - } else return true; -} - -/* class MessageAnswer*/ - -/** - * Create a new, empty answer message - */ -MessageAnswer::MessageAnswer() { - answer=ANSWER_ACK; -} - -/** - * Create a new answer message, with given ID and answer - * @param id Message ID - * @param ans Answer ID - * @throw std::runtime_error if message ID is incompatible with string data - */ -MessageAnswer::MessageAnswer(MessageID id, AnswerID ans) { - MessageAnswer::SetID(id); - MessageAnswer::SetAnswer(ans); -} - -/** - * Set message ID - * @param id Message ID - * @throw std::runtime_error if message ID is incompatible with answer message - */ -void MessageAnswer::SetID(MessageID id) { - if (CheckID(id)) - messageID = id; - else - throw std::runtime_error { - "Invalid message id for MessageAnswer" - }; -} - -/** - * Set message answer - * @param ans Answer ID - * @throw std::runtime_error if answer ID is incompatible with answer data - */ -void MessageAnswer::SetAnswer(AnswerID ans) { - if ((ans != ANSWER_ACK) && - (ans != ANSWER_NACK) && - (ans != ANSWER_LOST_ROBOT) && - (ans != ANSWER_ROBOT_CHECKSUM) && - (ans != ANSWER_ROBOT_ERROR) && - (ans != ANSWER_ROBOT_TIMEOUT) && - (ans != ANSWER_ROBOT_UNKNOWN_COMMAND)) { - this->answer = answer; - } else { - throw std::runtime_error{ - "Invalid answer for MessageAnswer"}; - } -} - -/** - * Translate content of message into a string that can be displayed - * @return A string describing message contents - */ -string MessageAnswer::ToString() { - if (CheckID(this->messageID)) - return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nAnswer: \"" + ANSWER_ID_STRING[this->answer] + "\""; - else - return "Invalid message"; -} - -/** - * Allocate a new message and copy contents of current message - * @return A message, copy of current - */ -Message* MessageAnswer::Copy() { - return new MessageAnswer(this->messageID, this->answer); -} - -/** - * Verify if message ID is compatible with current message type - * @param id Message ID - * @return true, if message ID is acceptable, false otherwise - */ -bool MessageAnswer::CheckID(MessageID id) { - if ((id != MESSAGE_ANSWER)) { + if (id != MESSAGE_CAM_IMAGE) { return false; } else return true; } @@ -523,7 +428,7 @@ string MessageBattery::ToString() { } if (CheckID(this->messageID)) - return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nBattery level: \"" + levelString + "\""; + return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nBattery level: \"" + levelString + "\""; else return "Invalid message"; } @@ -603,7 +508,7 @@ void MessagePosition::SetPosition(Position& pos) { */ string MessagePosition::ToString() { if (CheckID(this->messageID)) - return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nPosition: \"" + to_string(this->pos.center.x) + ";" + to_string(this->pos.center.y) + "\""; + return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nPosition: \"" + to_string(this->pos.center.x) + ";" + to_string(this->pos.center.y) + "\""; else return "Invalid message"; } @@ -622,91 +527,7 @@ Message* MessagePosition::Copy() { * @return true, if message ID is acceptable, false otherwise */ bool MessagePosition::CheckID(MessageID id) { - if ((id != MESSAGE_POSITION)) { + if ((id != MESSAGE_CAM_POSITION)) { return false; } else return true; } - - -/* class MessageState */ - -/** - * Create a new, empty state message - */ -MessageState::MessageState() { - state = ROBOT_NOT_BUSY; -} - -/** - * Create a new string message, with given ID and string - * @param id Message ID - * @param s Message string - * @throw std::runtime_error if message ID is incompatible with string data - */ -MessageState::MessageState(MessageID id, RobotState state) { - MessageState::SetID(id); - MessageState::SetState(state); -} - -/** - * Set message ID - * @param id Message ID - * @throw std::runtime_error if message ID is incompatible with robot state - */ -void MessageState::SetID(MessageID id) { - if (CheckID(id)) - messageID = id; - else - throw std::runtime_error { - "Invalid message id for MessageState" - }; -} - -/** - * Set robot state - * @param state Robot state - */ -void MessageState::SetState(RobotState state) { - if ((state != ROBOT_NOT_BUSY) && (state != ROBOT_BUSY)) { - throw std::runtime_error{ - "Invalid state for MessageState"}; - } else { - this->state = state; - } -} - -/** - * Translate content of message into a string that can be displayed - * @return A string describing message contents - */ -string MessageState::ToString() { - string stateString; - - if (this->state == ROBOT_NOT_BUSY) stateString="Not busy"; - else if (this->state == ROBOT_BUSY) stateString="Busy"; - else stateString="Invalid state"; - - if (CheckID(this->messageID)) - return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nState: \"" + stateString + "\""; - else - return "Invalid message"; -} - -/** - * Allocate a new message and copy contents of current message - * @return A message, copy of current - */ -Message* MessageState::Copy() { - return new MessageState(this->messageID, this->state); -} - -/** - * Verify if message ID is compatible with current message type - * @param id Message ID - * @return true, if message ID is acceptable, false otherwise - */ -bool MessageState::CheckID(MessageID id) { - if ((id != MESSAGE_ROBOT_CURRENT_STATE)) { - return false; - } else return true; -} \ No newline at end of file diff --git a/software/raspberry/superviseur-robot/lib/messages.h b/software/raspberry/superviseur-robot/lib/messages.h index d545220..60328de 100644 --- a/software/raspberry/superviseur-robot/lib/messages.h +++ b/software/raspberry/superviseur-robot/lib/messages.h @@ -33,11 +33,16 @@ typedef enum { MESSAGE_LOG, // Message containing answer (after robot command, or for monitor) - MESSAGE_ANSWER, + MESSAGE_ANSWER_ACK, + MESSAGE_ANSWER_NACK, + MESSAGE_ANSWER_ROBOT_TIMEOUT, + MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND, + MESSAGE_ANSWER_ROBOT_ERROR, + MESSAGE_ANSWER_COM_ERROR, // messages for serial communication with robot - MESSAGE_OPEN_COM, - MESSAGE_CLOSE_COM, + MESSAGE_ROBOT_COM_OPEN, + MESSAGE_ROBOT_COM_CLOSE, // Messages specific to server MESSAGE_MONITOR_LOST, @@ -45,13 +50,13 @@ typedef enum { // Messages for camera MESSAGE_CAM_OPEN, MESSAGE_CAM_CLOSE, - MESSAGE_ASK_ARENA, - MESSAGE_ARENA_CONFIRM, - MESSAGE_ARENA_INFIRM, - MESSAGE_COMPUTE_POSITION, - MESSAGE_STOP_COMPUTE_POSITION, - MESSAGE_POSITION, - MESSAGE_IMAGE, + MESSAGE_CAM_ASK_ARENA, + MESSAGE_CAM_ARENA_CONFIRM, + MESSAGE_CAM_ARENA_INFIRM, + MESSAGE_CAM_POSITION_COMPUTE_START, + MESSAGE_CAM_POSITION_COMPUTE_STOP, + MESSAGE_CAM_POSITION, + MESSAGE_CAM_IMAGE, // Messages for robot MESSAGE_ROBOT_PING, @@ -62,27 +67,18 @@ typedef enum { MESSAGE_ROBOT_MOVE, MESSAGE_ROBOT_TURN, MESSAGE_ROBOT_GO_FORWARD, - MESSAGE_ROBOT_GO_BACK, + MESSAGE_ROBOT_GO_BACKWARD, MESSAGE_ROBOT_GO_LEFT, MESSAGE_ROBOT_GO_RIGHT, MESSAGE_ROBOT_STOP, MESSAGE_ROBOT_POWEROFF, - MESSAGE_ROBOT_GET_BATTERY, + MESSAGE_ROBOT_BATTERY_GET, MESSAGE_ROBOT_BATTERY_LEVEL, - MESSAGE_ROBOT_GET_STATE, - MESSAGE_ROBOT_CURRENT_STATE + MESSAGE_ROBOT_STATE_GET, + MESSAGE_ROBOT_STATE_NOT_BUSY, + MESSAGE_ROBOT_STATE_BUSY } MessageID; -typedef enum { - ANSWER_ACK=0, - ANSWER_NACK, - ANSWER_LOST_ROBOT, - ANSWER_ROBOT_TIMEOUT, - ANSWER_ROBOT_UNKNOWN_COMMAND, - ANSWER_ROBOT_ERROR, - ANSWER_ROBOT_CHECKSUM -} AnswerID; - typedef enum { BATTERY_UNKNOWN=-1, BATTERY_EMPTY=0, @@ -90,11 +86,6 @@ typedef enum { BATTERY_FULL } BatteryLevel; -typedef enum { - ROBOT_NOT_BUSY=0, - ROBOT_BUSY -} RobotState; - using namespace std; /** @@ -132,6 +123,15 @@ public: */ virtual Message* Copy(); + /** + * Compare message ID + * @param id Id to compare message to + * @return true if id is equal to message id, false otherwise + */ + bool CompareID(MessageID id) { + return (this->messageID == id) ? true:false; + } + /** * Get message ID * @return Current message ID @@ -563,140 +563,5 @@ protected: bool CheckID(MessageID id); }; -/** - * Message class for holding answer, based on Message class - * - * @brief Answer message class - * - */ -class MessageAnswer : public Message { -public: - /** - * Create a new, empty image message - */ - MessageAnswer(); - - /** - * Create a new image message, with given ID and boolean value - * @param id Message ID - * @param ans Answer ID - * @throw std::runtime_error if message ID is incompatible with image message - */ - MessageAnswer(MessageID id, AnswerID ans); - - /** - * Set message ID - * @param id Message ID - * @throw std::runtime_error if message ID is incompatible withimage message - */ - void SetID(MessageID id); - - /** - * Get message image - * @return Pointer to image - */ - AnswerID GetAnswer() { - return answer; - } - - /** - * Set message answer - * @param ans Answer ID - */ - void SetAnswer(AnswerID ans); - - /** - * Translate content of message into a string that can be displayed - * @return A string describing message contents - */ - string ToString(); - - /** - * Allocate a new message and copy contents of current message - * @return A message, copy of current - */ - Message* Copy(); - -protected: - /** - * Message answer - */ - AnswerID answer; - - /** - * Verify if message ID is compatible with current message type - * @param id Message ID - * @return true, if message ID is acceptable, false otherwise - */ - bool CheckID(MessageID id); -}; - -/** - * Message class for holding robot state, based on Message class - * - * @brief Answer message class - * - */ -class MessageState: public Message { -public: - /** - * Create a new, empty image message - */ - MessageState(); - - /** - * Create a new image message, with given ID and boolean value - * @param id Message ID - * @param image Pointer to image - * @throw std::runtime_error if message ID is incompatible with image message - */ - MessageState(MessageID id, RobotState state); - - /** - * Set message ID - * @param id Message ID - * @throw std::runtime_error if message ID is incompatible withimage message - */ - void SetID(MessageID id); - - /** - * Get message image - * @return Pointer to image - */ - RobotState GetState() { - return state; - } - - /** - * Set message image - * @param image Pointer to image object - */ - void SetState(RobotState state); - - /** - * Translate content of message into a string that can be displayed - * @return A string describing message contents - */ - string ToString(); - - /** - * Allocate a new message and copy contents of current message - * @return A message, copy of current - */ - Message* Copy(); - -protected: - /** - * Robot state - */ - RobotState state; - - /** - * Verify if message ID is compatible with current message type - * @param id Message ID - * @return true, if message ID is acceptable, false otherwise - */ - bool CheckID(MessageID id); -}; #endif /* __MESSAGES_H__ */ diff --git a/software/raspberry/superviseur-robot/main.cpp b/software/raspberry/superviseur-robot/main.cpp index 7988a7a..de9d762 100644 --- a/software/raspberry/superviseur-robot/main.cpp +++ b/software/raspberry/superviseur-robot/main.cpp @@ -15,196 +15,39 @@ * along with this program. If not, see . */ -/** - * \file main.cpp - * \author PE.Hladik - * \version 1.0 - * \date 06/06/2017 - * \brief main program - */ - -#include -#include +#include #include #include -#include -#include -#include -#include -#include +#ifdef __WITH_PTHREAD__ +#include "tasks_pthread.h" +#else #include "tasks.h" - -// Déclaration des taches -RT_TASK th_server; -RT_TASK th_sendToMon; -RT_TASK th_receiveFromMon; -RT_TASK th_openComRobot; -RT_TASK th_startRobot; -RT_TASK th_move; - -// Déclaration des priorités des taches -int PRIORITY_TSERVER = 30; -int PRIORITY_TOPENCOMROBOT = 20; -int PRIORITY_TMOVE = 10; -int PRIORITY_TSENDTOMON = 25; -int PRIORITY_TRECEIVEFROMMON = 22; -int PRIORITY_TSTARTROBOT = 20; - -RT_MUTEX mutex_robotStarted; -RT_MUTEX mutex_move; - -// Déclaration des sémaphores -RT_SEM sem_barrier; -RT_SEM sem_openComRobot; -RT_SEM sem_serverOk; -RT_SEM sem_startRobot; - -// Déclaration des files de message -RT_QUEUE q_messageToMon; - -int MSG_QUEUE_SIZE = 10; - -// Déclaration des ressources partagées -int etatCommMoniteur = 1; -int robotStarted = 0; -char robotMove = DMB_STOP_MOVE; - -/** - * \fn void initStruct(void) - * \brief Initialisation des structures de l'application (tâches, mutex, - * semaphore, etc.) - */ -void initStruct(void); - -/** - * \fn void startTasks(void) - * \brief Démarrage des tâches - */ -void startTasks(void); - -/** - * \fn void deleteTasks(void) - * \brief Arrêt des tâches - */ -void deleteTasks(void); +#endif // __WITH_PTHREAD__ int main(int argc, char **argv) { - int err; + Tasks tasks; + //Lock the memory to avoid memory swapping for this program mlockall(MCL_CURRENT | MCL_FUTURE); - printf("#################################\n"); - printf("# DE STIJL PROJECT #\n"); - printf("#################################\n"); + cout<<"#################################"< - ./lib/camera.cpp + /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp ./lib/image.cpp - ./lib/img.cpp + /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp ./main.cpp ./lib/message.cpp ./lib/messages.cpp @@ -93,16 +93,12 @@ - - - - @@ -131,6 +127,11 @@ + + + + @@ -178,8 +184,6 @@ - - @@ -188,8 +192,6 @@ - - @@ -218,6 +220,11 @@ + + + + @@ -282,8 +294,6 @@ - - @@ -292,8 +302,6 @@ - - @@ -322,6 +330,11 @@ + + + + @@ -366,7 +384,7 @@ ./ ./lib - -D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables + -D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables -Wno-pmf-conversions _WITH_TRACE_ __FOR_PC__ @@ -382,8 +400,6 @@ - - @@ -392,11 +408,9 @@ - - - + @@ -404,15 +418,15 @@ - + - + - + @@ -420,6 +434,11 @@ + + + + diff --git a/software/raspberry/superviseur-robot/nbproject/private/configurations.xml b/software/raspberry/superviseur-robot/nbproject/private/configurations.xml index b55dcb1..8d4ae2f 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/configurations.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/configurations.xml @@ -116,6 +116,8 @@ + + diff --git a/software/raspberry/superviseur-robot/nbproject/private/private.xml b/software/raspberry/superviseur-robot/nbproject/private/private.xml index e64b33b..3f20efa 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/private.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/private.xml @@ -7,25 +7,15 @@ - file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/monitor.h file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.h - file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.h - file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.h - file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp - file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/messages.h - file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/robot.h - file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp - file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp - file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks_pthread.cpp - file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/image.h - file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/server.h - file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.h - file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks.cpp file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks_pthread.h - file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/message.h + file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.h file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/main.cpp + file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/messages.h + file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/messages.cpp + file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks_pthread.cpp diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index da732f2..43f9fea 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -15,16 +15,18 @@ * along with this program. If not, see . */ -/** - * \file functions.h - * \author PE.Hladik - * \version 1.0 - * \date 06/06/2017 - * \brief Miscellaneous functions used for destijl project. - */ - #include "tasks.h" +#ifndef __WITH_PTHREAD__ + +// Déclaration des priorités des taches +#define PRIORITY_TSERVER 30 +#define PRIORITY_TOPENCOMROBOT 20 +#define PRIORITY_TMOVE 10 +#define PRIORITY_TSENDTOMON 25 +#define PRIORITY_TRECEIVEFROMMON 22 +#define PRIORITY_TSTARTROBOT 20 + char mode_start; void write_in_queue(RT_QUEUE *, MessageToMon); @@ -258,4 +260,6 @@ void write_in_queue(RT_QUEUE *queue, MessageToMon msg) { buff = rt_queue_alloc(&q_messageToMon, sizeof (MessageToMon)); memcpy(buff, &msg, sizeof (MessageToMon)); rt_queue_send(&q_messageToMon, buff, sizeof (MessageToMon), Q_NORMAL); -} \ No newline at end of file +} + +#endif // __WITH_PTHREAD__ \ No newline at end of file diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index b4127b8..119fc7f 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -15,17 +15,10 @@ * along with this program. If not, see . */ -/** - * \file functions.h - * \author PE.Hladik - * \version 1.0 - * \date 06/06/2017 - * \brief Miscellaneous functions used for destijl project. - */ - #ifndef __TASKS_H__ #define __TASKS_H__ +#ifndef __WITH_PTHREAD__ #include #include #include @@ -37,71 +30,107 @@ #include #include -#include "monitor.h" -#include "robot.h" -#include "image.h" -#include "message.h" -#include "server.h" +//#include "monitor.h" +//#include "robot.h" +//#include "image.h" +//#include "message.h" +//#include "server.h" -extern RT_TASK th_server; -extern RT_TASK th_sendToMon; -extern RT_TASK th_receiveFromMon; -extern RT_TASK th_openComRobot; -extern RT_TASK th_startRobot; -extern RT_TASK th_move; +#include "messages.h" +#include "commonitor.h" +#include "comrobot.h" -extern RT_MUTEX mutex_robotStarted; -extern RT_MUTEX mutex_move; +using namespace std; -extern RT_SEM sem_barrier; -extern RT_SEM sem_openComRobot; -extern RT_SEM sem_serverOk; -extern RT_SEM sem_startRobot; +class Tasks { +public: + /** + * @brief Initialisation des structures de l'application (tâches, mutex, + * semaphore, etc.) + */ + void Init(); -extern RT_QUEUE q_messageToMon; + /** + * @brief Démarrage des tâches + */ + void Run(); -extern int etatCommMoniteur; -extern int robotStarted; -extern char robotMove; + /** + * @brief Arrêt des tâches + */ + void Stop(); + + /** + */ + void Join() { + rt_sem_broadcast(&sem_barrier); + pause(); + } + + /** + */ + bool AcceptClient() { + return false; + } + +private: + ComMonitor monitor; + ComRobot robot; + + RT_TASK th_server; + RT_TASK th_sendToMon; + RT_TASK th_receiveFromMon; + RT_TASK th_openComRobot; + RT_TASK th_startRobot; + RT_TASK th_move; -extern int MSG_QUEUE_SIZE; + RT_MUTEX mutex_robotStarted; + RT_MUTEX mutex_move; -extern int PRIORITY_TSERVER; -extern int PRIORITY_TOPENCOMROBOT; -extern int PRIORITY_TMOVE; -extern int PRIORITY_TSENDTOMON; -extern int PRIORITY_TRECEIVEFROMMON; -extern int PRIORITY_TSTARTROBOT; + RT_SEM sem_barrier; + RT_SEM sem_openComRobot; + RT_SEM sem_serverOk; + RT_SEM sem_startRobot; -/** - * \brief Thread handling server communication. - */ -void f_server(void *arg); + RT_QUEUE q_messageToMon; -/** - * \brief Thread handling communication to monitor. - */ -void f_sendToMon(void *arg); + int etatCommMoniteur; + int robotStarted; + char robotMove; -/** - * \brief Thread handling communication from monitor. - */ -void f_receiveFromMon(void *arg); + int MSG_QUEUE_SIZE; -/** - * \brief Thread handling opening of robot communication. - */ -void f_openComRobot(void * arg); + /** + * \brief Thread handling server communication. + */ + void f_server(void *arg); -/** - * \brief Thread handling robot mouvements. - */ -void f_move(void *arg); + /** + * \brief Thread handling communication to monitor. + */ + void f_sendToMon(void *arg); -/** - * \brief Thread handling robot activation. - */ -void f_startRobot(void *arg); + /** + * \brief Thread handling communication from monitor. + */ + void f_receiveFromMon(void *arg); -#endif /* FUNCTIONS_H */ + /** + * \brief Thread handling opening of robot communication. + */ + void f_openComRobot(void * arg); + + /** + * \brief Thread handling robot mouvements. + */ + void f_move(void *arg); + + /** + * \brief Thread handling robot activation. + */ + void f_startRobot(void *arg); +}; + +#endif // __WITH_PTHREAD__ +#endif // __TASKS_H__ diff --git a/software/raspberry/superviseur-robot/tasks_pthread.cpp b/software/raspberry/superviseur-robot/tasks_pthread.cpp index 6e00725..5cb92d8 100644 --- a/software/raspberry/superviseur-robot/tasks_pthread.cpp +++ b/software/raspberry/superviseur-robot/tasks_pthread.cpp @@ -15,249 +15,337 @@ * along with this program. If not, see . */ -/** - * \file functions.h - * \author PE.Hladik - * \version 1.0 - * \date 06/06/2017 - * \brief Miscellaneous functions used for destijl project. - */ - #include "tasks_pthread.h" +#include #ifdef __WITH_PTHREAD__ -char mode_start; -void write_in_queue(RT_QUEUE *, MessageToMon); +// Déclaration des priorités des taches +#define PRIORITY_TSERVER 30 +#define PRIORITY_TOPENCOMROBOT 20 +#define PRIORITY_TMOVE 10 +#define PRIORITY_TSENDTOMON 25 +#define PRIORITY_TRECEIVEFROMMON 22 +#define PRIORITY_TSTARTROBOT 20 -void f_server(void *arg) { +/* + * Some remarks: + * 1- This program is mostly a template. It shows you how to create tasks, semaphore + * message queues, mutex ... and how to use them + * + * 2- semDumber is, as name say, useless. Its goal is only to show you how to use semaphore + * + * 3- Data flow is probably not optimal + * + * 4- Take into account that ComRobot::Write will block your task when serial buffer is full, + * time for internal buffer to flush + * + * 5- Same behavior existe for ComMonitor::Write ! + * + * 6- When you want to write something in terminal, use cout and terminate with endl and flush + * + * 7- Good luck ! + */ + +void Tasks::Init() { + int status; + + /* Open com port with STM32 */ + cout << "Open serial com ("; + status = robot.Open("/dev/ttyUSB1"); + cout << status; + cout << ")" << endl; + + if (status >= 0) { + // Open server + + status = monitor.Open(SERVER_PORT); + cout << "Open server on port " << SERVER_PORT << " (" << status << ")" << endl; + + if (status < 0) throw std::runtime_error { + "Unable to start server on port " + std::to_string(SERVER_PORT) + }; + } else + throw std::runtime_error { + "Unable to open serial port /dev/ttyS0 " + }; +} + +void Tasks::Run() { + Message *msgRcv; + Message *msgSend; + int counter = 3; + + // threadServer=new thread((void (*)(void*)) &Tasks::ServerTask,this); + // threadSendToMon=new thread((void (*)(void*)) &Tasks::SendToMonTask,this); + // threadTimer=new thread((void (*)(void*)) &Tasks::TimerTask,this); + + msgSend = ComRobot::Ping(); + cout << "Send => " << msgSend->ToString() << endl << flush; + msgRcv = robot.SendCommand(msgSend, MESSAGE_ANSWER_ACK, 3); + cout << "Rcv <= " << msgRcv->ToString() << endl << flush; + + delete(msgRcv); + + msgSend = ComRobot::StartWithoutWD(); + cout << "Send => " << msgSend->ToString() << endl << flush; + msgRcv = robot.SendCommand(msgSend, MESSAGE_ANSWER_ACK, 3); + cout << "Rcv <= " << msgRcv->ToString() << endl << flush; + + delete(msgRcv); + + msgSend = ComRobot::Move(1000); + cout << "Send => " << msgSend->ToString() << endl << flush; + msgRcv = robot.SendCommand(msgSend, MESSAGE_ANSWER_ACK, 3); + cout << "Rcv <= " << msgRcv->ToString() << endl << flush; + + delete(msgRcv); + + msgSend = ComRobot::GetBattery(); + cout << "Send => " << msgSend->ToString() << endl << flush; + msgRcv = robot.SendCommand(msgSend, MESSAGE_ROBOT_BATTERY_LEVEL, 3); + cout << "Rcv <= " << msgRcv->ToString() << endl << flush; + + delete(msgRcv); +} + +void Tasks::Stop() { + monitor.Close(); + robot.Close(); +} + +void Tasks::ServerTask(void *arg) { int err; - /* INIT */ - RT_TASK_INFO info; - rt_task_inquire(NULL, &info); - printf("Init %s\n", info.name); - rt_sem_p(&sem_barrier, TM_INFINITE); - - err=openServer(DEFAULT_SERVER_PORT); - - if (err < 0) { - printf("Failed to start server: %s\n", strerror(-err)); - exit(EXIT_FAILURE); - } else { -#ifdef _WITH_TRACE_ - printf("%s: server started\n", info.name); -#endif - //Waiting for a client to connect - err=acceptClient(); + cout << "Start " << __PRETTY_FUNCTION__ <= 0) { -#ifdef _WITH_TRACE_ - printf("%s : message {%s,%s} in queue\n", info.name, msg.header, (char*)msg.data); -#endif - - send_message_to_monitor(msg.header, msg.data); - free_msgToMon_data(&msg); - rt_queue_free(&q_messageToMon, &msg); - } else { - printf("Error msg queue write: %s\n", strerror(-err)); + //std::this_thread::sleep_for(std::chrono::seconds ) + //sleep(1); + if (nanosleep(&tim, &tim2) < 0) { + printf("Nano sleep system call failed \n"); + return; } + + mutexTimer.unlock(); } } -void f_receiveFromMon(void *arg) { - MessageFromMon msg; - int err; - - /* INIT */ - RT_TASK_INFO info; - rt_task_inquire(NULL, &info); - printf("Init %s\n", info.name); - rt_sem_p(&sem_barrier, TM_INFINITE); - -#ifdef _WITH_TRACE_ - printf("%s : waiting for sem_serverOk\n", info.name); -#endif - rt_sem_p(&sem_serverOk, TM_INFINITE); - do { -#ifdef _WITH_TRACE_ - printf("%s : waiting for a message from monitor\n", info.name); -#endif - err = receive_message_from_monitor(msg.header, msg.data); -#ifdef _WITH_TRACE_ - printf("%s: msg {header:%s,data=%s} received from UI\n", info.name, msg.header, msg.data); -#endif - if (strcmp(msg.header, HEADER_MTS_COM_DMB) == 0) { - if (msg.data[0] == OPEN_COM_DMB) { // Open communication supervisor-robot -#ifdef _WITH_TRACE_ - printf("%s: message open Xbee communication\n", info.name); -#endif - rt_sem_v(&sem_openComRobot); - } - } else if (strcmp(msg.header, HEADER_MTS_DMB_ORDER) == 0) { - if (msg.data[0] == DMB_START_WITHOUT_WD) { // Start robot -#ifdef _WITH_TRACE_ - printf("%s: message start robot\n", info.name); -#endif - rt_sem_v(&sem_startRobot); - - } else if ((msg.data[0] == DMB_GO_BACK) - || (msg.data[0] == DMB_GO_FORWARD) - || (msg.data[0] == DMB_GO_LEFT) - || (msg.data[0] == DMB_GO_RIGHT) - || (msg.data[0] == DMB_STOP_MOVE)) { - - rt_mutex_acquire(&mutex_move, TM_INFINITE); - robotMove = msg.data[0]; - rt_mutex_release(&mutex_move); -#ifdef _WITH_TRACE_ - printf("%s: message update movement with %c\n", info.name, robotMove); -#endif - - } - } - } while (err > 0); - -} - -void f_openComRobot(void * arg) { - int err; - - /* INIT */ - RT_TASK_INFO info; - rt_task_inquire(NULL, &info); - printf("Init %s\n", info.name); - rt_sem_p(&sem_barrier, TM_INFINITE); - +void Tasks::SendToMonTask(void* arg) { + + cout << "Start " << __PRETTY_FUNCTION__ < open communication robot\n", info.name); -#endif - err = open_communication_robot(); - if (err == 0) { -#ifdef _WITH_TRACE_ - printf("%s : the communication is opened\n", info.name); -#endif - MessageToMon msg; - set_msgToMon_header(&msg, (char*)HEADER_STM_ACK); - write_in_queue(&q_messageToMon, msg); - } else { - MessageToMon msg; - set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK); - write_in_queue(&q_messageToMon, msg); - } + } } -void f_startRobot(void * arg) { - int err; +//void Tasks::f_sendToMon(void * arg) { +// int err; +// MessageToMon msg; +// +// /* INIT */ +// RT_TASK_INFO info; +// rt_task_inquire(NULL, &info); +// printf("Init %s\n", info.name); +// rt_sem_p(&sem_barrier, TM_INFINITE); +// +//#ifdef _WITH_TRACE_ +// printf("%s : waiting for sem_serverOk\n", info.name); +//#endif +// rt_sem_p(&sem_serverOk, TM_INFINITE); +// while (1) { +// +//#ifdef _WITH_TRACE_ +// printf("%s : waiting for a message in queue\n", info.name); +//#endif +// if (rt_queue_read(&q_messageToMon, &msg, sizeof (MessageToRobot), TM_INFINITE) >= 0) { +//#ifdef _WITH_TRACE_ +// printf("%s : message {%s,%s} in queue\n", info.name, msg.header, (char*)msg.data); +//#endif +// +// send_message_to_monitor(msg.header, msg.data); +// free_msgToMon_data(&msg); +// rt_queue_free(&q_messageToMon, &msg); +// } else { +// printf("Error msg queue write: %s\n", strerror(-err)); +// } +// } +//} +// +//void Tasks::f_receiveFromMon(void *arg) { +// MessageFromMon msg; +// int err; +// +// /* INIT */ +// RT_TASK_INFO info; +// rt_task_inquire(NULL, &info); +// printf("Init %s\n", info.name); +// rt_sem_p(&sem_barrier, TM_INFINITE); +// +//#ifdef _WITH_TRACE_ +// printf("%s : waiting for sem_serverOk\n", info.name); +//#endif +// rt_sem_p(&sem_serverOk, TM_INFINITE); +// do { +//#ifdef _WITH_TRACE_ +// printf("%s : waiting for a message from monitor\n", info.name); +//#endif +// err = receive_message_from_monitor(msg.header, msg.data); +//#ifdef _WITH_TRACE_ +// printf("%s: msg {header:%s,data=%s} received from UI\n", info.name, msg.header, msg.data); +//#endif +// if (strcmp(msg.header, HEADER_MTS_COM_DMB) == 0) { +// if (msg.data[0] == OPEN_COM_DMB) { // Open communication supervisor-robot +//#ifdef _WITH_TRACE_ +// printf("%s: message open Xbee communication\n", info.name); +//#endif +// rt_sem_v(&sem_openComRobot); +// } +// } else if (strcmp(msg.header, HEADER_MTS_DMB_ORDER) == 0) { +// if (msg.data[0] == DMB_START_WITHOUT_WD) { // Start robot +//#ifdef _WITH_TRACE_ +// printf("%s: message start robot\n", info.name); +//#endif +// rt_sem_v(&sem_startRobot); +// +// } else if ((msg.data[0] == DMB_GO_BACK) +// || (msg.data[0] == DMB_GO_FORWARD) +// || (msg.data[0] == DMB_GO_LEFT) +// || (msg.data[0] == DMB_GO_RIGHT) +// || (msg.data[0] == DMB_STOP_MOVE)) { +// +// rt_mutex_acquire(&mutex_move, TM_INFINITE); +// robotMove = msg.data[0]; +// rt_mutex_release(&mutex_move); +//#ifdef _WITH_TRACE_ +// printf("%s: message update movement with %c\n", info.name, robotMove); +//#endif +// +// } +// } +// } while (err > 0); +// +//} +// +//void Tasks::f_openComRobot(void * arg) { +// int err; +// +// /* INIT */ +// RT_TASK_INFO info; +// rt_task_inquire(NULL, &info); +// printf("Init %s\n", info.name); +// rt_sem_p(&sem_barrier, TM_INFINITE); +// +// while (1) { +//#ifdef _WITH_TRACE_ +// printf("%s : Wait sem_openComRobot\n", info.name); +//#endif +// rt_sem_p(&sem_openComRobot, TM_INFINITE); +//#ifdef _WITH_TRACE_ +// printf("%s : sem_openComRobot arrived => open communication robot\n", info.name); +//#endif +// err = open_communication_robot(); +// if (err == 0) { +//#ifdef _WITH_TRACE_ +// printf("%s : the communication is opened\n", info.name); +//#endif +// MessageToMon msg; +// set_msgToMon_header(&msg, (char*)HEADER_STM_ACK); +// write_in_queue(&q_messageToMon, msg); +// } else { +// MessageToMon msg; +// set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK); +// write_in_queue(&q_messageToMon, msg); +// } +// } +//} +// +//void Tasks::f_startRobot(void * arg) { +// int err; +// +// /* INIT */ +// RT_TASK_INFO info; +// rt_task_inquire(NULL, &info); +// printf("Init %s\n", info.name); +// rt_sem_p(&sem_barrier, TM_INFINITE); +// +// while (1) { +//#ifdef _WITH_TRACE_ +// printf("%s : Wait sem_startRobot\n", info.name); +//#endif +// rt_sem_p(&sem_startRobot, TM_INFINITE); +//#ifdef _WITH_TRACE_ +// printf("%s : sem_startRobot arrived => Start robot\n", info.name); +//#endif +// err = send_command_to_robot(DMB_START_WITHOUT_WD); +// if (err == 0) { +//#ifdef _WITH_TRACE_ +// printf("%s : the robot is started\n", info.name); +//#endif +// rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); +// robotStarted = 1; +// rt_mutex_release(&mutex_robotStarted); +// MessageToMon msg; +// set_msgToMon_header(&msg, (char*)HEADER_STM_ACK); +// write_in_queue(&q_messageToMon, msg); +// } else { +// MessageToMon msg; +// set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK); +// write_in_queue(&q_messageToMon, msg); +// } +// } +//} +// +//void Tasks::f_move(void *arg) { +// /* INIT */ +// RT_TASK_INFO info; +// rt_task_inquire(NULL, &info); +// printf("Init %s\n", info.name); +// rt_sem_p(&sem_barrier, TM_INFINITE); +// +// /* PERIODIC START */ +//#ifdef _WITH_PERIODIC_TRACE_ +// printf("%s: start period\n", info.name); +//#endif +// rt_task_set_periodic(NULL, TM_NOW, 100000000); +// while (1) { +//#ifdef _WITH_PERIODIC_TRACE_ +// printf("%s: Wait period \n", info.name); +//#endif +// rt_task_wait_period(NULL); +//#ifdef _WITH_PERIODIC_TRACE_ +// printf("%s: Periodic activation\n", info.name); +// printf("%s: move equals %c\n", info.name, robotMove); +//#endif +// rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); +// if (robotStarted) { +// rt_mutex_acquire(&mutex_move, TM_INFINITE); +// send_command_to_robot(robotMove); +// rt_mutex_release(&mutex_move); +//#ifdef _WITH_TRACE_ +// printf("%s: the movement %c was sent\n", info.name, robotMove); +//#endif +// } +// rt_mutex_release(&mutex_robotStarted); +// } +//} +// +//void write_in_queue(RT_QUEUE *queue, MessageToMon msg) { +// void *buff; +// buff = rt_queue_alloc(&q_messageToMon, sizeof (MessageToMon)); +// memcpy(buff, &msg, sizeof (MessageToMon)); +// rt_queue_send(&q_messageToMon, buff, sizeof (MessageToMon), Q_NORMAL); +//} - /* INIT */ - RT_TASK_INFO info; - rt_task_inquire(NULL, &info); - printf("Init %s\n", info.name); - rt_sem_p(&sem_barrier, TM_INFINITE); - - while (1) { -#ifdef _WITH_TRACE_ - printf("%s : Wait sem_startRobot\n", info.name); -#endif - rt_sem_p(&sem_startRobot, TM_INFINITE); -#ifdef _WITH_TRACE_ - printf("%s : sem_startRobot arrived => Start robot\n", info.name); -#endif - err = send_command_to_robot(DMB_START_WITHOUT_WD); - if (err == 0) { -#ifdef _WITH_TRACE_ - printf("%s : the robot is started\n", info.name); -#endif - rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); - robotStarted = 1; - rt_mutex_release(&mutex_robotStarted); - MessageToMon msg; - set_msgToMon_header(&msg, (char*)HEADER_STM_ACK); - write_in_queue(&q_messageToMon, msg); - } else { - MessageToMon msg; - set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK); - write_in_queue(&q_messageToMon, msg); - } - } -} - -void f_move(void *arg) { - /* INIT */ - RT_TASK_INFO info; - rt_task_inquire(NULL, &info); - printf("Init %s\n", info.name); - rt_sem_p(&sem_barrier, TM_INFINITE); - - /* PERIODIC START */ -#ifdef _WITH_PERIODIC_TRACE_ - printf("%s: start period\n", info.name); -#endif - rt_task_set_periodic(NULL, TM_NOW, 100000000); - while (1) { -#ifdef _WITH_PERIODIC_TRACE_ - printf("%s: Wait period \n", info.name); -#endif - rt_task_wait_period(NULL); -#ifdef _WITH_PERIODIC_TRACE_ - printf("%s: Periodic activation\n", info.name); - printf("%s: move equals %c\n", info.name, robotMove); -#endif - rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); - if (robotStarted) { - rt_mutex_acquire(&mutex_move, TM_INFINITE); - send_command_to_robot(robotMove); - rt_mutex_release(&mutex_move); -#ifdef _WITH_TRACE_ - printf("%s: the movement %c was sent\n", info.name, robotMove); -#endif - } - rt_mutex_release(&mutex_robotStarted); - } -} - -void write_in_queue(RT_QUEUE *queue, MessageToMon msg) { - void *buff; - buff = rt_queue_alloc(&q_messageToMon, sizeof (MessageToMon)); - memcpy(buff, &msg, sizeof (MessageToMon)); - rt_queue_send(&q_messageToMon, buff, sizeof (MessageToMon), Q_NORMAL); -} #endif //__WITH_PTHREAD__ \ No newline at end of file diff --git a/software/raspberry/superviseur-robot/tasks_pthread.h b/software/raspberry/superviseur-robot/tasks_pthread.h index 71dd7b4..b79c139 100644 --- a/software/raspberry/superviseur-robot/tasks_pthread.h +++ b/software/raspberry/superviseur-robot/tasks_pthread.h @@ -15,14 +15,6 @@ * along with this program. If not, see . */ -/** - * \file functions.h - * \author PE.Hladik - * \version 1.0 - * \date 06/06/2017 - * \brief Miscellaneous functions used for destijl project. - */ - #ifndef __TASKS_H__ #define __TASKS_H__ @@ -31,73 +23,112 @@ #include #include -#include +//#include "monitor.h" +//#include "robot.h" +//#include "image.h" +//#include "message.h" +//#include "server.h" -#include "monitor.h" -#include "robot.h" -#include "image.h" -#include "message.h" -#include "server.h" +#include "camera.h" +#include "img.h" -extern RT_TASK th_server; -extern RT_TASK th_sendToMon; -extern RT_TASK th_receiveFromMon; -extern RT_TASK th_openComRobot; -extern RT_TASK th_startRobot; -extern RT_TASK th_move; +#include "messages.h" +#include "commonitor.h" +#include "comrobot.h" -extern RT_MUTEX mutex_robotStarted; -extern RT_MUTEX mutex_move; - -extern RT_SEM sem_barrier; -extern RT_SEM sem_openComRobot; -extern RT_SEM sem_serverOk; -extern RT_SEM sem_startRobot; - -extern RT_QUEUE q_messageToMon; - -extern int etatCommMoniteur; -extern int robotStarted; -extern char robotMove; - -extern int MSG_QUEUE_SIZE; - -extern int PRIORITY_TSERVER; -extern int PRIORITY_TOPENCOMROBOT; -extern int PRIORITY_TMOVE; -extern int PRIORITY_TSENDTOMON; -extern int PRIORITY_TRECEIVEFROMMON; -extern int PRIORITY_TSTARTROBOT; +#include +#include +#include +class Tasks { +public: /** - * \brief Thread handling server communication. - */ -void f_server(void *arg); + * @brief Initialisation des structures de l'application (tâches, mutex, + * semaphore, etc.) + */ + void Init(); -/** - * \brief Thread handling communication to monitor. - */ -void f_sendToMon(void *arg); + /** + * @brief Démarrage des tâches + */ + void Run(); -/** - * \brief Thread handling communication from monitor. - */ -void f_receiveFromMon(void *arg); + /** + * @brief Arrêt des tâches + */ + void Stop(); + + /** + */ + void Join() { + threadServer->join(); + threadTimer->join(); + threadSendToMon->join(); + } + + /** + */ + bool AcceptClient() { + return monitor.AcceptClient(); + } + + /** + * @brief Thread handling server communication. + */ + void ServerTask(void *arg); -/** - * \brief Thread handling opening of robot communication. - */ -void f_openComRobot(void * arg); + /** + * @brief Thread handling server communication. + */ + void TimerTask(void *arg); + + /** + * @brief Thread handling communication to monitor. + */ + void SendToMonTask(void *arg); +private: + ComMonitor monitor; + ComRobot robot; + + thread *threadServer; + thread *threadSendToMon; + thread *threadTimer; +// thread *threadReceiveFromMon; +// thread *threadOpenComRobot; +// thread *threadStartRobot; +// thread *threadMove; +// thread *threadTimer; + + mutex mutexTimer; +// mutex mutexRobotStarted; +// mutex mutexMove; +// mutex semBarrier; +// mutex semOpenComRobot; +// mutex semServerOk; +// mutex semStartRobot; -/** - * \brief Thread handling robot mouvements. - */ -void f_move(void *arg); - -/** - * \brief Thread handling robot activation. - */ -void f_startRobot(void *arg); + +// +// /** +// * @brief Thread handling communication from monitor. +// */ +// void ReceiveFromMonTask(void *arg); +// +// /** +// * @brief Thread handling opening of robot communication. +// */ +// void OpenComRobotTask(void * arg); +// +// /** +// * @brief Thread handling robot mouvements. +// */ +// void MoveTask(void *arg); +// +// /** +// * @brief Thread handling robot activation. +// */ +// void StartRobotTask(void *arg); +}; #endif // __WITH_PTHREAD__ #endif /* __TASKS_H__ */ diff --git a/software/robot/src/cmdManager.c b/software/robot/src/cmdManager.c index 1e9c9d5..54e9732 100644 --- a/software/robot/src/cmdManager.c +++ b/software/robot/src/cmdManager.c @@ -340,7 +340,7 @@ void cmdStartWithoutWatchdogAction(void) { * Le type de commande à envoyer est :"M=val\r". Ou val * peut être positif ou negatif. * - * @param None + * @param NSTART_WITH_WDone * @retval None */ void cmdMoveAction(void) {