INSA/dumber
1
0
Fork 0
mirror of https://github.com/yoboujon/dumber.git synced 2025-06-08 22:00:49 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Classe comRobot partiellement debuggée

Browse source
This commit is contained in:
Sébastien DI MERCURIO 2018-12-21 16:36:52 +01:00
parent 34d1cb6bd8
commit 7f0be2d5fd
23 changed files with 1184 additions and 1387 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

BIN
software/raspberry/superviseur-robot/dist/Debug__Pthread_/GNU-Linux/superviseur-robot vendored Executable file
View file

Binary file not shown.

69
software/raspberry/superviseur-robot/lib/commonitor.cpp
View file

@ -33,7 +33,7 @@
*/ */
const string LABEL_MONITOR_ANSWER_ACK = "AACK"; const string LABEL_MONITOR_ANSWER_ACK = "AACK";
const string LABEL_MONITOR_ANSWER_NACK = "ANAK"; const string LABEL_MONITOR_ANSWER_NACK = "ANAK";
const string LABEL_MONITOR_ANSWER_LOST_DMB= "ATIM"; const string LABEL_MONITOR_ANSWER_COM_ERROR = "ACER";
const string LABEL_MONITOR_ANSWER_TIMEOUT = "ATIM"; const string LABEL_MONITOR_ANSWER_TIMEOUT = "ATIM";
const string LABEL_MONITOR_ANSWER_CMD_REJECTED = "ACRJ"; const string LABEL_MONITOR_ANSWER_CMD_REJECTED = "ACRJ";
const string LABEL_MONITOR_MESSAGE = "MSSG"; const string LABEL_MONITOR_MESSAGE = "MSSG";
@ -80,7 +80,7 @@ int ComMonitor::Open(int port) {
socketFD = socket(AF_INET, SOCK_STREAM, 0); socketFD = socket(AF_INET, SOCK_STREAM, 0);
if (socketFD < 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; server.sin_addr.s_addr = INADDR_ANY;
@ -88,7 +88,8 @@ int ComMonitor::Open(int port) {
server.sin_port = htons(port); server.sin_port = htons(port);
if (bind(socketFD, (struct sockaddr *) &server, sizeof (server)) < 0) { 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 ("<<to_string(port)<<")"<<endl<<flush;
throw std::runtime_error{"Can not bind socket"};
} }
listen(socketFD, 1); listen(socketFD, 1);
@ -117,9 +118,7 @@ int ComMonitor::AcceptClient() {
clientID = accept(socketFD, (struct sockaddr *) &client, (socklen_t*) & c); clientID = accept(socketFD, (struct sockaddr *) &client, (socklen_t*) & c);
if (clientID < 0) if (clientID < 0)
throw std::runtime_error { throw std::runtime_error {"Accept failed"};
"ComMonitor::AcceptClient : Accept failed"
};
return clientID; return clientID;
} }
@ -201,46 +200,42 @@ string ComMonitor::MessageToString(Message &msg) {
id = msg.GetID(); id = msg.GetID();
switch (id) { switch (id) {
case MESSAGE_ANSWER: case MESSAGE_ANSWER_ACK :
switch (((MessageAnswer*)localMsg)->GetAnswer()) {
case ANSWER_ACK:
str.append(LABEL_MONITOR_ANSWER_ACK); str.append(LABEL_MONITOR_ANSWER_ACK);
break; break;
case ANSWER_NACK: case MESSAGE_ANSWER_NACK:
str.append(LABEL_MONITOR_ANSWER_NACK); str.append(LABEL_MONITOR_ANSWER_NACK);
break; break;
case ANSWER_LOST_ROBOT: case MESSAGE_ANSWER_ROBOT_TIMEOUT:
str.append(LABEL_MONITOR_ANSWER_LOST_DMB);
break;
case ANSWER_ROBOT_TIMEOUT:
str.append(LABEL_MONITOR_ANSWER_TIMEOUT); str.append(LABEL_MONITOR_ANSWER_TIMEOUT);
break; break;
case ANSWER_ROBOT_UNKNOWN_COMMAND: case MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND:
str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED); str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED);
break; break;
case ANSWER_ROBOT_ERROR: case MESSAGE_ANSWER_ROBOT_ERROR:
str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED); str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED);
break; break;
default: case MESSAGE_ANSWER_COM_ERROR:
str.append(LABEL_MONITOR_ANSWER_NACK); str.append(LABEL_MONITOR_ANSWER_COM_ERROR);
};
break; break;
case MESSAGE_POSITION: case MESSAGE_CAM_POSITION:
pos = ((MessagePosition*) & msg)->GetPosition(); pos = ((MessagePosition*) & msg)->GetPosition();
str.append(LABEL_MONITOR_CAMERA_POSITION + LABEL_SEPARATOR_CHAR + to_string(pos.robotId) + ";" + 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.angle) + ";" + to_string(pos.center.x) + ";" + to_string(pos.center.y) + ";" +
to_string(pos.direction.x) + ";" + to_string(pos.direction.y)); to_string(pos.direction.x) + ";" + to_string(pos.direction.y));
break; break;
case MESSAGE_IMAGE: case MESSAGE_CAM_IMAGE:
str.append(LABEL_MONITOR_CAMERA_IMAGE + LABEL_SEPARATOR_CHAR + ((MessageImg*) & msg)->GetImage()->ToBase64()); str.append(LABEL_MONITOR_CAMERA_IMAGE + LABEL_SEPARATOR_CHAR + ((MessageImg*) & msg)->GetImage()->ToBase64());
break; break;
case MESSAGE_ROBOT_BATTERY_LEVEL: 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; break;
case MESSAGE_ROBOT_CURRENT_STATE: case MESSAGE_ROBOT_STATE_BUSY:
str.append(LABEL_MONITOR_ROBOT_CURRENT_STATE + LABEL_SEPARATOR_CHAR + to_string(((MessageState*) &msg)->GetState())); 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; break;
case MESSAGE_LOG: 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());
@ -249,10 +244,8 @@ string ComMonitor::MessageToString(Message &msg) {
str.append(""); //empty string str.append(""); //empty string
break; break;
default: default:
throw std::runtime_error cerr<<"["<<__PRETTY_FUNCTION__<<"] (from ComMonitor::Write): Invalid message to send ("<<msg.ToString()<<")"<<endl<<flush;
{ throw std::runtime_error {"Invalid message to send"};
"ComMonitor::MessageToString (from ComMonitor::Write): Invalid message to send (" + msg.ToString()
};
} }
str.append("\n"); str.append("\n");
@ -295,33 +288,33 @@ Message *ComMonitor::StringToMessage(string &s) {
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_RESET) != string::npos) { } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_RESET) != string::npos) {
msg = new Message(MESSAGE_ROBOT_RESET); msg = new Message(MESSAGE_ROBOT_RESET);
} else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_ASK) != string::npos) { } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_ASK) != string::npos) {
msg = new Message(MESSAGE_ASK_ARENA); msg = new Message(MESSAGE_CAM_ASK_ARENA);
} else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_CONFIRM) != string::npos) { } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_CONFIRM) != string::npos) {
msg = new Message(MESSAGE_ARENA_CONFIRM); msg = new Message(MESSAGE_CAM_ARENA_CONFIRM);
} else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_INFIRME) != string::npos) { } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_INFIRME) != string::npos) {
msg = new Message(MESSAGE_ARENA_INFIRM); msg = new Message(MESSAGE_CAM_ARENA_INFIRM);
} else if (tokenCmd.find(LABEL_MONITOR_CAMERA_CLOSE) != string::npos) { } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_CLOSE) != string::npos) {
msg = new Message(MESSAGE_CAM_CLOSE); msg = new Message(MESSAGE_CAM_CLOSE);
} else if (tokenCmd.find(LABEL_MONITOR_CAMERA_OPEN) != string::npos) { } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_OPEN) != string::npos) {
msg = new Message(MESSAGE_CAM_OPEN); msg = new Message(MESSAGE_CAM_OPEN);
} else if (tokenCmd.find(LABEL_MONITOR_CAMERA_POSITION_COMPUTE) != string::npos) { } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_POSITION_COMPUTE) != string::npos) {
msg = new Message(MESSAGE_COMPUTE_POSITION); msg = new Message(MESSAGE_CAM_POSITION_COMPUTE_START);
} else if (tokenCmd.find(LABEL_MONITOR_CAMERA_POSITION_STOP) != string::npos) { } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_POSITION_STOP) != string::npos) {
msg = new Message(MESSAGE_STOP_COMPUTE_POSITION); msg = new Message(MESSAGE_CAM_POSITION_COMPUTE_STOP);
} else if (tokenCmd.find(LABEL_MONITOR_MESSAGE) != string::npos) { } else if (tokenCmd.find(LABEL_MONITOR_MESSAGE) != string::npos) {
msg = new MessageString(MESSAGE_LOG, tokenData); msg = new MessageString(MESSAGE_LOG, tokenData);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_COM_CLOSE) != string::npos) { } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_COM_CLOSE) != string::npos) {
msg = new Message(MESSAGE_CLOSE_COM); msg = new Message(MESSAGE_ROBOT_COM_CLOSE);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_COM_OPEN) != string::npos) { } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_COM_OPEN) != string::npos) {
msg = new Message(MESSAGE_OPEN_COM); msg = new Message(MESSAGE_ROBOT_COM_OPEN);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GET_BATTERY) != string::npos) { } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GET_BATTERY) != string::npos) {
msg = new Message(MESSAGE_ROBOT_GET_BATTERY); msg = new Message(MESSAGE_ROBOT_BATTERY_GET);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GET_STATE) != string::npos) { } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GET_STATE) != string::npos) {
msg = new Message(MESSAGE_ROBOT_GET_STATE); msg = new Message(MESSAGE_ROBOT_STATE_GET);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_FORWARD) != string::npos) { } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_FORWARD) != string::npos) {
msg = new Message(MESSAGE_ROBOT_GO_FORWARD); msg = new Message(MESSAGE_ROBOT_GO_FORWARD);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_BACKWARD) != string::npos) { } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_BACKWARD) != string::npos) {
msg = new Message(MESSAGE_ROBOT_GO_BACK); msg = new Message(MESSAGE_ROBOT_GO_BACKWARD);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_LEFT) != string::npos) { } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_LEFT) != string::npos) {
msg = new Message(MESSAGE_ROBOT_GO_LEFT); msg = new Message(MESSAGE_ROBOT_GO_LEFT);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_RIGHT) != string::npos) { } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_RIGHT) != string::npos) {

2
software/raspberry/superviseur-robot/lib/commonitor.h
View file

@ -23,6 +23,8 @@
using namespace std; using namespace std;
#define SERVER_PORT 1234
/** /**
* Class used for generating a server and communicating through it with monitor * Class used for generating a server and communicating through it with monitor
* *

488
software/raspberry/superviseur-robot/lib/comrobot.cpp
View file

@ -35,16 +35,23 @@
/* /*
* Constants to be used for communicating with robot. Contains command tag * Constants to be used for communicating with robot. Contains command tag
*/ */
typedef enum { const char LABEL_ROBOT_PING = 'p';
LABEL_ANGLE_POSITION = 'p', const char LABEL_ROBOT_RESET = 'r';
LABEL_ANGULAR_SPEED = 's', const char LABEL_ROBOT_START_WITH_WD = 'W';
LABEL_BATTERY_LEVEL = 'b', const char LABEL_ROBOT_START_WITHOUT_WD = 'u';
LABEL_BETA_ANGLE = 'v', const char LABEL_ROBOT_RELOAD_WD = 'w';
LABEL_USER_PRESENCE = 'u', 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';
LABEL_TORQUE = 'c', const char LABEL_ROBOT_OK = 'O';
LABEL_EMERGENCY_STOP = 'a' const char LABEL_ROBOT_ERROR = 'E';
} LabelRobot; 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 * Open serial link with robot
@ -52,22 +59,35 @@ typedef enum {
* @throw std::runtime_error if it fails * @throw std::runtime_error if it fails
*/ */
int ComRobot::Open() { int ComRobot::Open() {
fd = open(USART_FILENAME, O_RDWR | O_NOCTTY /*| O_NDELAY*/); //Open in blocking read/write mode return this->Open(USART_FILENAME);
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"};
exit(EXIT_FAILURE);
} }
//Configuration of the serial port 115 520 Bauds /**
* 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; struct termios options;
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); tcgetattr(fd, &options);
options.c_cflag = B115200 | CS8 | CLOCAL | CREAD; //<Set baud rate options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
options.c_iflag = IGNPAR; // ignores bytes with bad parity cfsetospeed (&options, B9600);
options.c_oflag = 0; cfsetispeed (&options, B9600);
options.c_lflag = 0; options.c_cc[VMIN]=0;
tcflush(fd, TCIFLUSH); options.c_cc[VTIME]=1; /* Timeout of 100 ms per character*/
tcsetattr(fd, TCSANOW, &options); tcsetattr(fd, TCSANOW, &options);
}
return fd; return fd;
} }
@ -79,185 +99,6 @@ int ComRobot::Open() {
int ComRobot::Close() { int ComRobot::Close() {
return close(fd); return close(fd);
} }
/**
* 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* ComRobot::Read() {
int rxLength;
unsigned char rxBuffer[6];
unsigned char receivedChar;
bool messageComplete = false;
Message *msg;
unsigned int i;
/* Call pre method for read */
Read_Pre();
/* a message is composed of 7 bytes.
the byte 0 should always be '<'
the byte 1 should be an ascii char that is the label. It define what the data represent
the bytes 2 to 5 are the float value
the byte 6 should always be a '\n'
*/
while (messageComplete == false) {
rxLength = read(this->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 * Send a message to robot
* @param msg Message to send 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) * @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 * @warning Write is not thread save : check that multiple tasks can't access this method simultaneously
*/ */
int ComRobot::Write(Message* msg) { Message *ComRobot::Write(Message* msg) {
unsigned char buffer[7]; Message *msgAnswer;
int ret_val = 0; string s;
MessageToChar(msg, buffer); if (this->fd != -1) {
Write_Pre(); Write_Pre();
if (this->fd != -1) { s=MessageToString(msg);
int count = write(this->fd, &buffer[0], 7); //Filestream, bytes to write, number of bytes to write AddChecksum(s);
//cout << "[" <<__PRETTY_FUNCTION__<<"] Send command: "<<s<<endl<<flush;
int count = write(this->fd, s.c_str(), s.length()); //Filestream, bytes to write, number of bytes to write
if (count < 0) { if (count < 0) {
printf("Warning: UART TX error in ComStm32::Write\n"); 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 = "<<s<<endl<<flush;
if (VerifyChecksum(s)) {
msgAnswer = StringToMessage(s);
} else msgAnswer = new Message(MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND);
} catch (std::runtime_error &e) {
s = string(e.what());
if (s.find("imeout")) { // timeout detecté
msgAnswer = new Message(MESSAGE_ANSWER_ROBOT_TIMEOUT);
} else { } else {
ret_val = 1; msgAnswer = new Message(MESSAGE_ANSWER_COM_ERROR);
} }
} }
}
} else {
cerr << __PRETTY_FUNCTION__ << ": Com port not open" << endl << flush;
throw std::runtime_error{"Com port not open"};
}
// deallocation of msg // deallocation of msg
delete(msg); delete(msg);
Write_Post(); return msgAnswer;
}
return ret_val; /**
* 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
*/
string ComRobot::Read() {
string s;
int rxLength;
unsigned char receivedChar;
do {
rxLength = read(this->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;
} }
/** /**
@ -296,39 +250,107 @@ int ComRobot::Write(Message* msg) {
* @param msg Message to be sent to robot * @param msg Message to be sent to robot
* @param buffer Array of char, image of message to send * @param buffer Array of char, image of message to send
*/ */
void ComRobot::MessageToChar(Message *msg, unsigned char *buffer) { string ComRobot::MessageToString(Message *msg) {
string s;
float val_f; float val_f;
int val_i; int val_i;
unsigned char *b; unsigned char *b;
buffer[0] = '<';
buffer[6] = '\n';
switch (msg->GetID()) { switch (msg->GetID()) {
case MESSAGE_TORQUE: case MESSAGE_ROBOT_PING:
buffer[1] = LABEL_TORQUE; s+=LABEL_ROBOT_PING;
val_f = ((MessageFloat*) msg)->GetValue();
b = (unsigned char *) &val_f;
break; break;
case MESSAGE_EMERGENCY_STOP: case MESSAGE_ROBOT_RESET:
buffer[1] = LABEL_EMERGENCY_STOP; s+=LABEL_ROBOT_RESET;
if (((MessageBool*) msg)->GetState()) break;
val_i = 1; case MESSAGE_ROBOT_POWEROFF:
else s+=LABEL_ROBOT_POWEROFF;
val_i = 0; break;
b = (unsigned char *) &val_i; 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; break;
default: default:
printf("Invalid message to send"); cerr<<"["<<__PRETTY_FUNCTION__<<"] Invalid message for robot ("<<msg->ToString()<<")"<<endl<<flush;
val_i = 0; throw std::runtime_error {"Invalid message"};
b = (unsigned char *) &val_i;
} }
buffer[2] = b[0]; return s;
buffer[3] = b[1];
buffer[4] = b[2];
buffer[5] = b[3];
} }
/**
* 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;
}

161
software/raspberry/superviseur-robot/lib/comrobot.h
View file

@ -19,6 +19,7 @@
#define __COMROBOT_H__ #define __COMROBOT_H__
#include "messages.h" #include "messages.h"
#include <string>
using namespace std; using namespace std;
@ -30,15 +31,18 @@ using namespace std;
*/ */
class ComRobot { class ComRobot {
public: public:
/** /**
* Constructor * Constructor
*/ */
ComRobot() {} ComRobot() {
}
/** /**
* Destructor * Destructor
*/ */
virtual ~ComRobot() {} virtual ~ComRobot() {
}
/** /**
* Open serial link with robot * Open serial link with robot
@ -47,56 +51,106 @@ public:
*/ */
int Open(); 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 * Close serial link
* @return Success if above 0, failure if below 0 * @return Success if above 0, failure if below 0
*/ */
int Close(); 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 * Send a message to robot
* @param msg Message to send to robot * @param msg Message to send to robot
* @return 1 if success, 0 otherwise * @return A message containing either an answer (Ack/Nak/Timeout/Error) or a value (battery level, robot state) depending of the command
* @attention Message is destroyed (delete) after being sent. You do not need to delete it yourself * @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) * @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 * @warning Write is not thread save : check that multiple tasks can't access this method simultaneously
*/ */
int Write(Message* msg); Message *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() {}
/** /**
* Function called at beginning of Write method * Function called at beginning of Write method
* Use it to do some synchronization (call of mutex, for example) * 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 * Function called at end of Write method
* Use it to do some synchronization (call of mutex, for example) * 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: protected:
/** /**
@ -105,39 +159,40 @@ protected:
int fd; int fd;
/** /**
* Convert an array of char to float * Get an answer from robot
* @param bytes Array of char, containing a binary image of a float * @return String containing answer from robot
* @return Float value * @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 * Convert a string to its message representation (when receiving data from robot)
* @param bytes Array of char, containing a binary image of a boolean * @param s String from robot containing answer
* @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
* @return Message corresponding to received array of char * @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 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__ */ #endif /* __COMROBOT_H__ */

20
software/raspberry/superviseur-robot/lib/img.cpp
View file

@ -27,7 +27,11 @@ Img::Img(ImageMat imgMatrice) {
} }
string Img::ToString() { 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() { Img* Img::Copy() {
@ -100,13 +104,13 @@ Jpg Img::toJpg() {
return imgJpg; return imgJpg;
} }
string Img::ToBase64() { //string Img::ToBase64() {
string imgBase64; // string imgBase64;
Jpg imgJpg = toJpg(); // Jpg imgJpg = toJpg();
//
/* faire la convertion Jpg vers base 64 */ // /* faire la convertion Jpg vers base 64 */
return imgBase64; // return imgBase64;
} //}
std::list<Position> Img::search_robot(Arene monArene) { std::list<Position> Img::search_robot(Arene monArene) {

2
software/raspberry/superviseur-robot/lib/img.h
View file

@ -49,7 +49,7 @@ typedef struct {
class Arene { class Arene {
public: public:
Arene(); Arene() {}
cv::Rect arene; cv::Rect arene;
bool empty(); bool empty();

231
software/raspberry/superviseur-robot/lib/messages.cpp
View file

@ -27,7 +27,12 @@
const string MESSAGE_ID_STRING[] = { const string MESSAGE_ID_STRING[] = {
"Empty", "Empty",
"Log", "Log",
"Answer", "Answer [Acknowledge]",
"Answer [Not Acknowledge]",
"Answer [Command timeout]",
"Answer [Command unknown]",
"Answer [Command error]",
"Answer [Communication error]",
"Monitor connection lost", "Monitor connection lost",
"Open serial com", "Open serial com",
"Close serial com", "Close serial com",
@ -42,8 +47,8 @@ const string MESSAGE_ID_STRING[] = {
"Image", "Image",
"Robot ping", "Robot ping",
"Robot reset", "Robot reset",
"Robot start with wtachdog", "Robot start with watchdog",
"Robot start without wtachdog", "Robot start without watchdog",
"Robot reload watchdog", "Robot reload watchdog",
"Robot move", "Robot move",
"Robot turn", "Robot turn",
@ -56,7 +61,9 @@ const string MESSAGE_ID_STRING[] = {
"Robot get battery", "Robot get battery",
"Robot battery level", "Robot battery level",
"Robot get state", "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() { string Message::ToString() {
if (CheckID(this->messageID)) if (CheckID(this->messageID))
return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\""; return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"";
else else
return "Invalid message"; return "Invalid message";
} }
@ -118,7 +125,7 @@ string Message::ToString() {
* @return A message, copy of current * @return A message, copy of current
*/ */
Message* Message::Copy() { Message* Message::Copy() {
Message *msg = new Message(); Message *msg = new Message(this->messageID);
return msg; return msg;
} }
@ -128,30 +135,12 @@ Message* Message::Copy() {
* @return Current message ID * @return Current message ID
*/ */
bool Message::CheckID(MessageID id) { bool Message::CheckID(MessageID id) {
if ((id != MESSAGE_EMPTY) && if ((id == MESSAGE_CAM_IMAGE) ||
(id != MESSAGE_MONITOR_LOST) && (id == MESSAGE_CAM_POSITION) ||
(id != MESSAGE_ARENA_CONFIRM) && (id == MESSAGE_ROBOT_MOVE) ||
(id != MESSAGE_ARENA_INFIRM) && (id == MESSAGE_ROBOT_TURN) ||
(id != MESSAGE_ASK_ARENA) && (id == MESSAGE_LOG) ||
(id != MESSAGE_CAM_CLOSE) && (id == MESSAGE_ROBOT_BATTERY_LEVEL)) {
(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)) {
return false; return false;
} else return true; } else return true;
} }
@ -197,7 +186,7 @@ void MessageInt::SetID(MessageID id) {
*/ */
string MessageInt::ToString() { string MessageInt::ToString() {
if (CheckID(this->messageID)) 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 else
return "Invalid message"; return "Invalid message";
} }
@ -263,7 +252,7 @@ void MessageString::SetID(MessageID id) {
*/ */
string MessageString::ToString() { string MessageString::ToString() {
if (CheckID(this->messageID)) 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 else
return "Invalid message"; return "Invalid message";
} }
@ -342,7 +331,7 @@ void MessageImg::SetImage(Img* image) {
*/ */
string MessageImg::ToString() { string MessageImg::ToString() {
if (CheckID(this->messageID)) 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 else
return "Invalid message"; return "Invalid message";
} }
@ -362,91 +351,7 @@ Message* MessageImg::Copy() {
* @return true, if message ID is acceptable, false otherwise * @return true, if message ID is acceptable, false otherwise
*/ */
bool MessageImg::CheckID(MessageID id) { bool MessageImg::CheckID(MessageID id) {
if (id != MESSAGE_IMAGE) { if (id != MESSAGE_CAM_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)) {
return false; return false;
} else return true; } else return true;
} }
@ -523,7 +428,7 @@ string MessageBattery::ToString() {
} }
if (CheckID(this->messageID)) 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 else
return "Invalid message"; return "Invalid message";
} }
@ -603,7 +508,7 @@ void MessagePosition::SetPosition(Position& pos) {
*/ */
string MessagePosition::ToString() { string MessagePosition::ToString() {
if (CheckID(this->messageID)) 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 else
return "Invalid message"; return "Invalid message";
} }
@ -622,91 +527,7 @@ Message* MessagePosition::Copy() {
* @return true, if message ID is acceptable, false otherwise * @return true, if message ID is acceptable, false otherwise
*/ */
bool MessagePosition::CheckID(MessageID id) { 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; return false;
} else return true; } else return true;
} }

193
software/raspberry/superviseur-robot/lib/messages.h
View file

@ -33,11 +33,16 @@ typedef enum {
MESSAGE_LOG, MESSAGE_LOG,
// Message containing answer (after robot command, or for monitor) // 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 // messages for serial communication with robot
MESSAGE_OPEN_COM, MESSAGE_ROBOT_COM_OPEN,
MESSAGE_CLOSE_COM, MESSAGE_ROBOT_COM_CLOSE,
// Messages specific to server // Messages specific to server
MESSAGE_MONITOR_LOST, MESSAGE_MONITOR_LOST,
@ -45,13 +50,13 @@ typedef enum {
// Messages for camera // Messages for camera
MESSAGE_CAM_OPEN, MESSAGE_CAM_OPEN,
MESSAGE_CAM_CLOSE, MESSAGE_CAM_CLOSE,
MESSAGE_ASK_ARENA, MESSAGE_CAM_ASK_ARENA,
MESSAGE_ARENA_CONFIRM, MESSAGE_CAM_ARENA_CONFIRM,
MESSAGE_ARENA_INFIRM, MESSAGE_CAM_ARENA_INFIRM,
MESSAGE_COMPUTE_POSITION, MESSAGE_CAM_POSITION_COMPUTE_START,
MESSAGE_STOP_COMPUTE_POSITION, MESSAGE_CAM_POSITION_COMPUTE_STOP,
MESSAGE_POSITION, MESSAGE_CAM_POSITION,
MESSAGE_IMAGE, MESSAGE_CAM_IMAGE,
// Messages for robot // Messages for robot
MESSAGE_ROBOT_PING, MESSAGE_ROBOT_PING,
@ -62,27 +67,18 @@ typedef enum {
MESSAGE_ROBOT_MOVE, MESSAGE_ROBOT_MOVE,
MESSAGE_ROBOT_TURN, MESSAGE_ROBOT_TURN,
MESSAGE_ROBOT_GO_FORWARD, MESSAGE_ROBOT_GO_FORWARD,
MESSAGE_ROBOT_GO_BACK, MESSAGE_ROBOT_GO_BACKWARD,
MESSAGE_ROBOT_GO_LEFT, MESSAGE_ROBOT_GO_LEFT,
MESSAGE_ROBOT_GO_RIGHT, MESSAGE_ROBOT_GO_RIGHT,
MESSAGE_ROBOT_STOP, MESSAGE_ROBOT_STOP,
MESSAGE_ROBOT_POWEROFF, MESSAGE_ROBOT_POWEROFF,
MESSAGE_ROBOT_GET_BATTERY, MESSAGE_ROBOT_BATTERY_GET,
MESSAGE_ROBOT_BATTERY_LEVEL, MESSAGE_ROBOT_BATTERY_LEVEL,
MESSAGE_ROBOT_GET_STATE, MESSAGE_ROBOT_STATE_GET,
MESSAGE_ROBOT_CURRENT_STATE MESSAGE_ROBOT_STATE_NOT_BUSY,
MESSAGE_ROBOT_STATE_BUSY
} MessageID; } 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 { typedef enum {
BATTERY_UNKNOWN=-1, BATTERY_UNKNOWN=-1,
BATTERY_EMPTY=0, BATTERY_EMPTY=0,
@ -90,11 +86,6 @@ typedef enum {
BATTERY_FULL BATTERY_FULL
} BatteryLevel; } BatteryLevel;
typedef enum {
ROBOT_NOT_BUSY=0,
ROBOT_BUSY
} RobotState;
using namespace std; using namespace std;
/** /**
@ -132,6 +123,15 @@ public:
*/ */
virtual Message* Copy(); 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 * Get message ID
* @return Current message ID * @return Current message ID
@ -563,140 +563,5 @@ protected:
bool CheckID(MessageID id); 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__ */ #endif /* __MESSAGES_H__ */

199
software/raspberry/superviseur-robot/main.cpp
View file

@ -15,196 +15,39 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>. * along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
/** #include <iostream>
* \file main.cpp
* \author PE.Hladik
* \version 1.0
* \date 06/06/2017
* \brief main program
*/
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h> #include <unistd.h>
#include <sys/mman.h> #include <sys/mman.h>
#include <alchemy/task.h>
#include <alchemy/timer.h>
#include <alchemy/mutex.h>
#include <alchemy/sem.h>
#include <alchemy/queue.h>
#ifdef __WITH_PTHREAD__
#include "tasks_pthread.h"
#else
#include "tasks.h" #include "tasks.h"
#endif // __WITH_PTHREAD__
// 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);
int main(int argc, char **argv) { int main(int argc, char **argv) {
int err; Tasks tasks;
//Lock the memory to avoid memory swapping for this program //Lock the memory to avoid memory swapping for this program
mlockall(MCL_CURRENT | MCL_FUTURE); mlockall(MCL_CURRENT | MCL_FUTURE);
printf("#################################\n"); cout<<"#################################"<<endl;
printf("# DE STIJL PROJECT #\n"); cout<<"# DE STIJL PROJECT #"<<endl;
printf("#################################\n"); cout<<"#################################"<<endl;
initStruct(); tasks.Init();
startTasks();
rt_sem_broadcast(&sem_barrier); /*if (tasks.AcceptClient()) {
pause(); tasks.Run();
deleteTasks();
tasks.Join();
}
tasks.Stop();*/
tasks.Run();
return 0; return 0;
} }
void initStruct(void) {
int err;
/* Creation des mutex */
if (err = rt_mutex_create(&mutex_robotStarted, NULL)) {
printf("Error mutex create: %d %s\n", err, strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_move, NULL)) {
printf("Error mutex create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
/* Creation du semaphore */
if (err = rt_sem_create(&sem_barrier, "truc", 0, S_FIFO)) {
printf("Error semaphore create 1: %d %s\n", err, strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_openComRobot, NULL, 0, S_FIFO)) {
printf("Error semaphore create 2: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_serverOk, NULL, 0, S_FIFO)) {
printf("Error semaphore create 3: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_startRobot, NULL, 0, S_FIFO)) {
printf("Error semaphore create 4: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
/* Creation des taches */
if (err = rt_task_create(&th_server, "th_server", 0, PRIORITY_TSERVER, 0)) {
printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_receiveFromMon, "th_receiveFromMon", 0, PRIORITY_TRECEIVEFROMMON, 0)) {
printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_sendToMon, "th_sendToMon", 0, PRIORITY_TSENDTOMON, 0)) {
printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_openComRobot, "th_openComRobot", 0, PRIORITY_TOPENCOMROBOT, 0)) {
printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_startRobot, "th_startRobot", 0, PRIORITY_TSTARTROBOT, 0)) {
printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0)) {
printf("Error task create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
/* Creation des files de messages */
if (err = rt_queue_create(&q_messageToMon, "toto", MSG_QUEUE_SIZE * sizeof (MessageToRobot), MSG_QUEUE_SIZE, Q_FIFO)) {
printf("Error msg queue create: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
}
void startTasks() {
int err;
if (err = rt_task_start(&th_startRobot, &f_startRobot, NULL)) {
printf("Error task start: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_receiveFromMon, &f_receiveFromMon, NULL)) {
printf("Error task start: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_sendToMon, &f_sendToMon, NULL)) {
printf("Error task start: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_openComRobot, &f_openComRobot, NULL)) {
printf("Error task start: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_move, &f_move, NULL)) {
printf("Error task start: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_server, &f_server, NULL)) {
printf("Error task start: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
}
void deleteTasks() {
rt_task_delete(&th_server);
rt_task_delete(&th_openComRobot);
rt_task_delete(&th_move);
}

12
software/raspberry/superviseur-robot/nbproject/Makefile-Debug.mk
View file

@ -42,8 +42,10 @@ OBJECTFILES= \
${OBJECTDIR}/lib/server.o \ ${OBJECTDIR}/lib/server.o \
${OBJECTDIR}/main.o \ ${OBJECTDIR}/main.o \
${OBJECTDIR}/tasks.o \ ${OBJECTDIR}/tasks.o \
${OBJECTDIR}/_ext/6cc0dc4a/camera.o \
${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \ ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \
${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \ ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \
${OBJECTDIR}/_ext/6cc0dc4a/img.o \
${OBJECTDIR}/tasks_pthread.o ${OBJECTDIR}/tasks_pthread.o
@ -106,6 +108,11 @@ ${OBJECTDIR}/tasks.o: tasks.cpp
${RM} "$@.d" ${RM} "$@.d"
$(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/tasks.o tasks.cpp $(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/tasks.o tasks.cpp
${OBJECTDIR}/_ext/6cc0dc4a/camera.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp
${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
${RM} "$@.d"
$(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/camera.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp
${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp
${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a ${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
${RM} "$@.d" ${RM} "$@.d"
@ -116,6 +123,11 @@ ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o: /home/dimercur/Documents/Travail/git/dumb
${RM} "$@.d" ${RM} "$@.d"
$(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp $(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp
${OBJECTDIR}/_ext/6cc0dc4a/img.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp
${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
${RM} "$@.d"
$(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/img.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp
${OBJECTDIR}/tasks_pthread.o: tasks_pthread.cpp ${OBJECTDIR}/tasks_pthread.o: tasks_pthread.cpp
${MKDIR} -p ${OBJECTDIR} ${MKDIR} -p ${OBJECTDIR}
${RM} "$@.d" ${RM} "$@.d"

40
software/raspberry/superviseur-robot/nbproject/Makefile-Debug__Pthread_.mk
View file

@ -35,14 +35,12 @@ OBJECTDIR=${CND_BUILDDIR}/${CND_CONF}/${CND_PLATFORM}
# Object Files # Object Files
OBJECTFILES= \ OBJECTFILES= \
${OBJECTDIR}/lib/message.o \
${OBJECTDIR}/lib/messages.o \ ${OBJECTDIR}/lib/messages.o \
${OBJECTDIR}/lib/monitor.o \
${OBJECTDIR}/lib/robot.o \
${OBJECTDIR}/lib/server.o \
${OBJECTDIR}/main.o \ ${OBJECTDIR}/main.o \
${OBJECTDIR}/_ext/6cc0dc4a/camera.o \
${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \ ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \
${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \ ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \
${OBJECTDIR}/_ext/6cc0dc4a/img.o \
${OBJECTDIR}/tasks_pthread.o ${OBJECTDIR}/tasks_pthread.o
@ -50,8 +48,8 @@ OBJECTFILES= \
CFLAGS=-I/usr/xenomai/include/mercury -I/usr/xenomai/include -D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables -D__MERCURY__ -I/usr/xenomai/include/alchemy CFLAGS=-I/usr/xenomai/include/mercury -I/usr/xenomai/include -D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables -D__MERCURY__ -I/usr/xenomai/include/alchemy
# CC Compiler Flags # CC Compiler Flags
CCFLAGS=-D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables CCFLAGS=-D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables -Wno-pmf-conversions
CXXFLAGS=-D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables CXXFLAGS=-D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables -Wno-pmf-conversions
# Fortran Compiler Flags # Fortran Compiler Flags
FFLAGS= FFLAGS=
@ -70,36 +68,21 @@ ${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}/superviseur-robot: ${OBJECTFILES}
${MKDIR} -p ${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM} ${MKDIR} -p ${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}
${LINK.cc} -o ${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}/superviseur-robot ${OBJECTFILES} ${LDLIBSOPTIONS} -lpthread -lrt ${LINK.cc} -o ${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}/superviseur-robot ${OBJECTFILES} ${LDLIBSOPTIONS} -lpthread -lrt
${OBJECTDIR}/lib/message.o: lib/message.cpp
${MKDIR} -p ${OBJECTDIR}/lib
${RM} "$@.d"
$(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/lib/message.o lib/message.cpp
${OBJECTDIR}/lib/messages.o: lib/messages.cpp ${OBJECTDIR}/lib/messages.o: lib/messages.cpp
${MKDIR} -p ${OBJECTDIR}/lib ${MKDIR} -p ${OBJECTDIR}/lib
${RM} "$@.d" ${RM} "$@.d"
$(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/lib/messages.o lib/messages.cpp $(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/lib/messages.o lib/messages.cpp
${OBJECTDIR}/lib/monitor.o: lib/monitor.cpp
${MKDIR} -p ${OBJECTDIR}/lib
${RM} "$@.d"
$(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/lib/monitor.o lib/monitor.cpp
${OBJECTDIR}/lib/robot.o: lib/robot.cpp
${MKDIR} -p ${OBJECTDIR}/lib
${RM} "$@.d"
$(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/lib/robot.o lib/robot.cpp
${OBJECTDIR}/lib/server.o: lib/server.cpp
${MKDIR} -p ${OBJECTDIR}/lib
${RM} "$@.d"
$(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/lib/server.o lib/server.cpp
${OBJECTDIR}/main.o: main.cpp ${OBJECTDIR}/main.o: main.cpp
${MKDIR} -p ${OBJECTDIR} ${MKDIR} -p ${OBJECTDIR}
${RM} "$@.d" ${RM} "$@.d"
$(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/main.o main.cpp $(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/main.o main.cpp
${OBJECTDIR}/_ext/6cc0dc4a/camera.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp
${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
${RM} "$@.d"
$(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/camera.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp
${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp
${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a ${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
${RM} "$@.d" ${RM} "$@.d"
@ -110,6 +93,11 @@ ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o: /home/dimercur/Documents/Travail/git/dumb
${RM} "$@.d" ${RM} "$@.d"
$(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp $(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp
${OBJECTDIR}/_ext/6cc0dc4a/img.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp
${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
${RM} "$@.d"
$(COMPILE.cc) -g -D_WITH_TRACE_ -D__FOR_PC__ -D__WITH_PTHREAD__ -I./ -I./lib `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/img.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp
${OBJECTDIR}/tasks_pthread.o: tasks_pthread.cpp ${OBJECTDIR}/tasks_pthread.o: tasks_pthread.cpp
${MKDIR} -p ${OBJECTDIR} ${MKDIR} -p ${OBJECTDIR}
${RM} "$@.d" ${RM} "$@.d"

12
software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk
View file

@ -43,8 +43,10 @@ OBJECTFILES= \
${OBJECTDIR}/lib/server.o \ ${OBJECTDIR}/lib/server.o \
${OBJECTDIR}/main.o \ ${OBJECTDIR}/main.o \
${OBJECTDIR}/tasks.o \ ${OBJECTDIR}/tasks.o \
${OBJECTDIR}/_ext/6cc0dc4a/camera.o \
${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \ ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \
${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \ ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \
${OBJECTDIR}/_ext/6cc0dc4a/img.o \
${OBJECTDIR}/tasks_pthread.o ${OBJECTDIR}/tasks_pthread.o
@ -112,6 +114,11 @@ ${OBJECTDIR}/tasks.o: tasks.cpp
${RM} "$@.d" ${RM} "$@.d"
$(COMPILE.cc) -g -D_WITH_TRACE_ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/tasks.o tasks.cpp $(COMPILE.cc) -g -D_WITH_TRACE_ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/tasks.o tasks.cpp
${OBJECTDIR}/_ext/6cc0dc4a/camera.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp
${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
${RM} "$@.d"
$(COMPILE.cc) -g -D_WITH_TRACE_ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/camera.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp
${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp
${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a ${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
${RM} "$@.d" ${RM} "$@.d"
@ -122,6 +129,11 @@ ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o: /home/dimercur/Documents/Travail/git/dumb
${RM} "$@.d" ${RM} "$@.d"
$(COMPILE.cc) -g -D_WITH_TRACE_ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp $(COMPILE.cc) -g -D_WITH_TRACE_ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp
${OBJECTDIR}/_ext/6cc0dc4a/img.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp
${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
${RM} "$@.d"
$(COMPILE.cc) -g -D_WITH_TRACE_ -I./ -I./lib -I/usr/xenomai/include -I/usr/xenomai/include/mercury `pkg-config --cflags opencv` -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/img.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp
${OBJECTDIR}/tasks_pthread.o: tasks_pthread.cpp ${OBJECTDIR}/tasks_pthread.o: tasks_pthread.cpp
${MKDIR} -p ${OBJECTDIR} ${MKDIR} -p ${OBJECTDIR}
${RM} "$@.d" ${RM} "$@.d"

12
software/raspberry/superviseur-robot/nbproject/Makefile-Release.mk
View file

@ -43,8 +43,10 @@ OBJECTFILES= \
${OBJECTDIR}/lib/server.o \ ${OBJECTDIR}/lib/server.o \
${OBJECTDIR}/main.o \ ${OBJECTDIR}/main.o \
${OBJECTDIR}/tasks.o \ ${OBJECTDIR}/tasks.o \
${OBJECTDIR}/_ext/6cc0dc4a/camera.o \
${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \ ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \
${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \ ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \
${OBJECTDIR}/_ext/6cc0dc4a/img.o \
${OBJECTDIR}/tasks_pthread.o ${OBJECTDIR}/tasks_pthread.o
@ -112,6 +114,11 @@ ${OBJECTDIR}/tasks.o: tasks.cpp
${RM} "$@.d" ${RM} "$@.d"
$(COMPILE.cc) -O2 -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/tasks.o tasks.cpp $(COMPILE.cc) -O2 -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/tasks.o tasks.cpp
${OBJECTDIR}/_ext/6cc0dc4a/camera.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp
${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
${RM} "$@.d"
$(COMPILE.cc) -O2 -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/camera.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp
${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp
${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a ${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
${RM} "$@.d" ${RM} "$@.d"
@ -122,6 +129,11 @@ ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o: /home/dimercur/Documents/Travail/git/dumb
${RM} "$@.d" ${RM} "$@.d"
$(COMPILE.cc) -O2 -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp $(COMPILE.cc) -O2 -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp
${OBJECTDIR}/_ext/6cc0dc4a/img.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp
${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
${RM} "$@.d"
$(COMPILE.cc) -O2 -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/_ext/6cc0dc4a/img.o /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp
${OBJECTDIR}/tasks_pthread.o: tasks_pthread.cpp ${OBJECTDIR}/tasks_pthread.o: tasks_pthread.cpp
${MKDIR} -p ${OBJECTDIR} ${MKDIR} -p ${OBJECTDIR}
${RM} "$@.d" ${RM} "$@.d"

70
software/raspberry/superviseur-robot/nbproject/configurations.xml
View file

@ -26,11 +26,11 @@
<logicalFolder name="SourceFiles" <logicalFolder name="SourceFiles"
displayName="Source Files" displayName="Source Files"
projectFiles="true"> projectFiles="true">
<itemPath>./lib/camera.cpp</itemPath> <itemPath>/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp</itemPath>
<itemPath>/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp</itemPath> <itemPath>/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp</itemPath>
<itemPath>/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp</itemPath> <itemPath>/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp</itemPath>
<itemPath>./lib/image.cpp</itemPath> <itemPath>./lib/image.cpp</itemPath>
<itemPath>./lib/img.cpp</itemPath> <itemPath>/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp</itemPath>
<itemPath>./main.cpp</itemPath> <itemPath>./main.cpp</itemPath>
<itemPath>./lib/message.cpp</itemPath> <itemPath>./lib/message.cpp</itemPath>
<itemPath>./lib/messages.cpp</itemPath> <itemPath>./lib/messages.cpp</itemPath>
@ -93,16 +93,12 @@
</compileType> </compileType>
<item path="./gdbsudo.sh" ex="false" tool="3" flavor2="0"> <item path="./gdbsudo.sh" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/camera.cpp" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/camera.h" ex="false" tool="3" flavor2="0"> <item path="./lib/camera.h" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/definitions.h" ex="false" tool="3" flavor2="0"> <item path="./lib/definitions.h" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/image.h" ex="false" tool="3" flavor2="0"> <item path="./lib/image.h" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/img.cpp" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/img.h" ex="false" tool="3" flavor2="0"> <item path="./lib/img.h" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/message.cpp" ex="false" tool="1" flavor2="0"> <item path="./lib/message.cpp" ex="false" tool="1" flavor2="0">
@ -131,6 +127,11 @@
</item> </item>
<item path="./tasks.h" ex="false" tool="3" flavor2="0"> <item path="./tasks.h" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp"
ex="false"
tool="1"
flavor2="0">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp" <item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp"
ex="false" ex="false"
tool="1" tool="1"
@ -151,6 +152,11 @@
tool="3" tool="3"
flavor2="0"> flavor2="0">
</item> </item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp"
ex="false"
tool="1"
flavor2="0">
</item>
<item path="tasks_pthread.cpp" ex="false" tool="1" flavor2="0"> <item path="tasks_pthread.cpp" ex="false" tool="1" flavor2="0">
</item> </item>
<item path="tasks_pthread.h" ex="false" tool="3" flavor2="0"> <item path="tasks_pthread.h" ex="false" tool="3" flavor2="0">
@ -178,8 +184,6 @@
</compileType> </compileType>
<item path="./gdbsudo.sh" ex="false" tool="3" flavor2="0"> <item path="./gdbsudo.sh" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/camera.cpp" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/camera.h" ex="false" tool="3" flavor2="0"> <item path="./lib/camera.h" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/definitions.h" ex="false" tool="3" flavor2="0"> <item path="./lib/definitions.h" ex="false" tool="3" flavor2="0">
@ -188,8 +192,6 @@
</item> </item>
<item path="./lib/image.h" ex="false" tool="3" flavor2="0"> <item path="./lib/image.h" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/img.cpp" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/img.h" ex="false" tool="3" flavor2="0"> <item path="./lib/img.h" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/message.cpp" ex="false" tool="1" flavor2="0"> <item path="./lib/message.cpp" ex="false" tool="1" flavor2="0">
@ -218,6 +220,11 @@
</item> </item>
<item path="./tasks.h" ex="false" tool="3" flavor2="0"> <item path="./tasks.h" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp"
ex="false"
tool="1"
flavor2="0">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp" <item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp"
ex="false" ex="false"
tool="1" tool="1"
@ -238,6 +245,11 @@
tool="3" tool="3"
flavor2="0"> flavor2="0">
</item> </item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp"
ex="false"
tool="1"
flavor2="0">
</item>
<item path="tasks_pthread.cpp" ex="false" tool="1" flavor2="0"> <item path="tasks_pthread.cpp" ex="false" tool="1" flavor2="0">
</item> </item>
<item path="tasks_pthread.h" ex="false" tool="3" flavor2="0"> <item path="tasks_pthread.h" ex="false" tool="3" flavor2="0">
@ -282,8 +294,6 @@
</compileType> </compileType>
<item path="./gdbsudo.sh" ex="false" tool="3" flavor2="0"> <item path="./gdbsudo.sh" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/camera.cpp" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/camera.h" ex="false" tool="3" flavor2="0"> <item path="./lib/camera.h" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/definitions.h" ex="false" tool="3" flavor2="0"> <item path="./lib/definitions.h" ex="false" tool="3" flavor2="0">
@ -292,8 +302,6 @@
</item> </item>
<item path="./lib/image.h" ex="false" tool="3" flavor2="0"> <item path="./lib/image.h" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/img.cpp" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/img.h" ex="false" tool="3" flavor2="0"> <item path="./lib/img.h" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/message.cpp" ex="false" tool="1" flavor2="0"> <item path="./lib/message.cpp" ex="false" tool="1" flavor2="0">
@ -322,6 +330,11 @@
</item> </item>
<item path="./tasks.h" ex="false" tool="3" flavor2="0"> <item path="./tasks.h" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp"
ex="false"
tool="1"
flavor2="0">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp" <item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp"
ex="false" ex="false"
tool="1" tool="1"
@ -342,6 +355,11 @@
tool="3" tool="3"
flavor2="0"> flavor2="0">
</item> </item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp"
ex="false"
tool="1"
flavor2="0">
</item>
<item path="tasks_pthread.cpp" ex="false" tool="1" flavor2="0"> <item path="tasks_pthread.cpp" ex="false" tool="1" flavor2="0">
</item> </item>
<item path="tasks_pthread.h" ex="false" tool="3" flavor2="0"> <item path="tasks_pthread.h" ex="false" tool="3" flavor2="0">
@ -366,7 +384,7 @@
<pElem>./</pElem> <pElem>./</pElem>
<pElem>./lib</pElem> <pElem>./lib</pElem>
</incDir> </incDir>
<commandLine>-D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables</commandLine> <commandLine>-D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables -Wno-pmf-conversions</commandLine>
<preprocessorList> <preprocessorList>
<Elem>_WITH_TRACE_</Elem> <Elem>_WITH_TRACE_</Elem>
<Elem>__FOR_PC__</Elem> <Elem>__FOR_PC__</Elem>
@ -382,8 +400,6 @@
</compileType> </compileType>
<item path="./gdbsudo.sh" ex="false" tool="3" flavor2="0"> <item path="./gdbsudo.sh" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/camera.cpp" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/camera.h" ex="false" tool="3" flavor2="0"> <item path="./lib/camera.h" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/definitions.h" ex="false" tool="3" flavor2="0"> <item path="./lib/definitions.h" ex="false" tool="3" flavor2="0">
@ -392,11 +408,9 @@
</item> </item>
<item path="./lib/image.h" ex="false" tool="3" flavor2="0"> <item path="./lib/image.h" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/img.cpp" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/img.h" ex="false" tool="3" flavor2="0"> <item path="./lib/img.h" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/message.cpp" ex="false" tool="1" flavor2="9"> <item path="./lib/message.cpp" ex="true" tool="1" flavor2="9">
</item> </item>
<item path="./lib/message.h" ex="false" tool="3" flavor2="0"> <item path="./lib/message.h" ex="false" tool="3" flavor2="0">
</item> </item>
@ -404,15 +418,15 @@
</item> </item>
<item path="./lib/messages.h" ex="false" tool="3" flavor2="0"> <item path="./lib/messages.h" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/monitor.cpp" ex="false" tool="1" flavor2="9"> <item path="./lib/monitor.cpp" ex="true" tool="1" flavor2="9">
</item> </item>
<item path="./lib/monitor.h" ex="false" tool="3" flavor2="0"> <item path="./lib/monitor.h" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/robot.cpp" ex="false" tool="1" flavor2="9"> <item path="./lib/robot.cpp" ex="true" tool="1" flavor2="9">
</item> </item>
<item path="./lib/robot.h" ex="false" tool="3" flavor2="0"> <item path="./lib/robot.h" ex="false" tool="3" flavor2="0">
</item> </item>
<item path="./lib/server.cpp" ex="false" tool="1" flavor2="9"> <item path="./lib/server.cpp" ex="true" tool="1" flavor2="9">
</item> </item>
<item path="./lib/server.h" ex="false" tool="3" flavor2="0"> <item path="./lib/server.h" ex="false" tool="3" flavor2="0">
</item> </item>
@ -420,6 +434,11 @@
</item> </item>
<item path="./tasks.cpp" ex="true" tool="1" flavor2="9"> <item path="./tasks.cpp" ex="true" tool="1" flavor2="9">
</item> </item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp"
ex="false"
tool="1"
flavor2="0">
</item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp" <item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp"
ex="false" ex="false"
tool="1" tool="1"
@ -440,6 +459,11 @@
tool="3" tool="3"
flavor2="0"> flavor2="0">
</item> </item>
<item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp"
ex="false"
tool="1"
flavor2="0">
</item>
<item path="tasks_pthread.cpp" ex="false" tool="1" flavor2="9"> <item path="tasks_pthread.cpp" ex="false" tool="1" flavor2="9">
</item> </item>
<item path="tasks_pthread.h" ex="false" tool="3" flavor2="0"> <item path="tasks_pthread.h" ex="false" tool="3" flavor2="0">

2
software/raspberry/superviseur-robot/nbproject/private/configurations.xml
View file

@ -116,6 +116,8 @@
<gdb_interceptlist> <gdb_interceptlist>
<gdbinterceptoptions gdb_all="false" gdb_unhandled="true" gdb_unexpected="true"/> <gdbinterceptoptions gdb_all="false" gdb_unhandled="true" gdb_unexpected="true"/>
</gdb_interceptlist> </gdb_interceptlist>
<gdb_signals>
</gdb_signals>
<gdb_options> <gdb_options>
<DebugOptions> <DebugOptions>
</DebugOptions> </DebugOptions>

18
software/raspberry/superviseur-robot/nbproject/private/private.xml
View file

@ -7,25 +7,15 @@
<editor-bookmarks xmlns="http://www.netbeans.org/ns/editor-bookmarks/2" lastBookmarkId="0"/> <editor-bookmarks xmlns="http://www.netbeans.org/ns/editor-bookmarks/2" lastBookmarkId="0"/>
<open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2"> <open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2">
<group> <group>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/monitor.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp</file> <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.h</file> <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/messages.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/robot.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks_pthread.cpp</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/image.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/server.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks.cpp</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks_pthread.h</file> <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks_pthread.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/message.h</file> <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/main.cpp</file> <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/main.cpp</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/messages.h</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/messages.cpp</file> <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/messages.cpp</file>
<file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks_pthread.cpp</file>
</group> </group>
</open-files> </open-files>
</project-private> </project-private>

20
software/raspberry/superviseur-robot/tasks.cpp
View file

@ -15,16 +15,18 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>. * along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
/**
* \file functions.h
* \author PE.Hladik
* \version 1.0
* \date 06/06/2017
* \brief Miscellaneous functions used for destijl project.
*/
#include "tasks.h" #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; char mode_start;
void write_in_queue(RT_QUEUE *, MessageToMon); void write_in_queue(RT_QUEUE *, MessageToMon);
@ -259,3 +261,5 @@ void write_in_queue(RT_QUEUE *queue, MessageToMon msg) {
memcpy(buff, &msg, sizeof (MessageToMon)); memcpy(buff, &msg, sizeof (MessageToMon));
rt_queue_send(&q_messageToMon, buff, sizeof (MessageToMon), Q_NORMAL); rt_queue_send(&q_messageToMon, buff, sizeof (MessageToMon), Q_NORMAL);
} }
#endif // __WITH_PTHREAD__

103
software/raspberry/superviseur-robot/tasks.h
View file

@ -15,17 +15,10 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>. * along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
/**
* \file functions.h
* \author PE.Hladik
* \version 1.0
* \date 06/06/2017
* \brief Miscellaneous functions used for destijl project.
*/
#ifndef __TASKS_H__ #ifndef __TASKS_H__
#define __TASKS_H__ #define __TASKS_H__
#ifndef __WITH_PTHREAD__
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
#include <unistd.h> #include <unistd.h>
@ -37,41 +30,75 @@
#include <alchemy/sem.h> #include <alchemy/sem.h>
#include <alchemy/queue.h> #include <alchemy/queue.h>
#include "monitor.h" //#include "monitor.h"
#include "robot.h" //#include "robot.h"
#include "image.h" //#include "image.h"
#include "message.h" //#include "message.h"
#include "server.h" //#include "server.h"
extern RT_TASK th_server; #include "messages.h"
extern RT_TASK th_sendToMon; #include "commonitor.h"
extern RT_TASK th_receiveFromMon; #include "comrobot.h"
extern RT_TASK th_openComRobot;
extern RT_TASK th_startRobot;
extern RT_TASK th_move;
extern RT_MUTEX mutex_robotStarted; using namespace std;
extern RT_MUTEX mutex_move;
extern RT_SEM sem_barrier; class Tasks {
extern RT_SEM sem_openComRobot; public:
extern RT_SEM sem_serverOk; /**
extern RT_SEM sem_startRobot; * @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; * @brief Arrêt des tâches
extern char robotMove; */
void Stop();
extern int MSG_QUEUE_SIZE; /**
*/
void Join() {
rt_sem_broadcast(&sem_barrier);
pause();
}
extern int PRIORITY_TSERVER; /**
extern int PRIORITY_TOPENCOMROBOT; */
extern int PRIORITY_TMOVE; bool AcceptClient() {
extern int PRIORITY_TSENDTOMON; return false;
extern int PRIORITY_TRECEIVEFROMMON; }
extern int PRIORITY_TSTARTROBOT;
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;
RT_MUTEX mutex_robotStarted;
RT_MUTEX mutex_move;
RT_SEM sem_barrier;
RT_SEM sem_openComRobot;
RT_SEM sem_serverOk;
RT_SEM sem_startRobot;
RT_QUEUE q_messageToMon;
int etatCommMoniteur;
int robotStarted;
char robotMove;
int MSG_QUEUE_SIZE;
/** /**
* \brief Thread handling server communication. * \brief Thread handling server communication.
@ -102,6 +129,8 @@ void f_move(void *arg);
* \brief Thread handling robot activation. * \brief Thread handling robot activation.
*/ */
void f_startRobot(void *arg); void f_startRobot(void *arg);
};
#endif /* FUNCTIONS_H */ #endif // __WITH_PTHREAD__
#endif // __TASKS_H__

532
software/raspberry/superviseur-robot/tasks_pthread.cpp
View file

@ -15,249 +15,337 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>. * along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
/**
* \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 "tasks_pthread.h"
#include <time.h>
#ifdef __WITH_PTHREAD__ #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; int err;
/* INIT */ cout << "Start " << __PRETTY_FUNCTION__ <<endl<<flush;
RT_TASK_INFO info;
rt_task_inquire(NULL, &info);
printf("Init %s\n", info.name);
rt_sem_p(&sem_barrier, TM_INFINITE);
err=openServer(DEFAULT_SERVER_PORT);
if (err < 0) {
printf("Failed to start server: %s\n", strerror(-err));
exit(EXIT_FAILURE);
} else {
#ifdef _WITH_TRACE_
printf("%s: server started\n", info.name);
#endif
//Waiting for a client to connect
err=acceptClient();
if (err<0) {
printf("Client accept failed: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
#ifdef _WITH_TRACE_
printf ("client connected: %d\n", err);
printf ("Rock'n'roll baby !\n");
#endif
rt_sem_broadcast(&sem_serverOk);
}
}
void f_sendToMon(void * arg) {
int err;
MessageToMon msg;
/* INIT */
RT_TASK_INFO info;
rt_task_inquire(NULL, &info);
printf("Init %s\n", info.name);
rt_sem_p(&sem_barrier, TM_INFINITE);
#ifdef _WITH_TRACE_
printf("%s : waiting for sem_serverOk\n", info.name);
#endif
rt_sem_p(&sem_serverOk, TM_INFINITE);
while (1) { 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::TimerTask(void* arg) {
struct timespec tim, tim2;
tim.tv_sec = 0;
tim.tv_nsec = 100000000;
cout << "Start " << __PRETTY_FUNCTION__ <<endl<<flush;
while (1) {
//std::this_thread::sleep_for(std::chrono::seconds )
//sleep(1);
if (nanosleep(&tim, &tim2) < 0) {
printf("Nano sleep system call failed \n");
return;
} }
void f_receiveFromMon(void *arg) { mutexTimer.unlock();
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::SendToMonTask(void* arg) {
void f_openComRobot(void * arg) { cout << "Start " << __PRETTY_FUNCTION__ <<endl<<flush;
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) { while (1) {
#ifdef _WITH_TRACE_
printf("%s : Wait sem_openComRobot\n", info.name);
#endif
rt_sem_p(&sem_openComRobot, TM_INFINITE);
#ifdef _WITH_TRACE_
printf("%s : sem_openComRobot arrived => open communication robot\n", info.name);
#endif
err = open_communication_robot();
if (err == 0) {
#ifdef _WITH_TRACE_
printf("%s : the communication is opened\n", info.name);
#endif
MessageToMon msg;
set_msgToMon_header(&msg, (char*)HEADER_STM_ACK);
write_in_queue(&q_messageToMon, msg);
} else {
MessageToMon msg;
set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK);
write_in_queue(&q_messageToMon, msg);
}
} }
} }
void f_startRobot(void * arg) { //void Tasks::f_sendToMon(void * arg) {
int err; // 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__ #endif //__WITH_PTHREAD__

137
software/raspberry/superviseur-robot/tasks_pthread.h
View file

@ -15,14 +15,6 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>. * along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
/**
* \file functions.h
* \author PE.Hladik
* \version 1.0
* \date 06/06/2017
* \brief Miscellaneous functions used for destijl project.
*/
#ifndef __TASKS_H__ #ifndef __TASKS_H__
#define __TASKS_H__ #define __TASKS_H__
@ -31,73 +23,112 @@
#include <stdlib.h> #include <stdlib.h>
#include <unistd.h> #include <unistd.h>
#include <sys/mman.h> //#include "monitor.h"
//#include "robot.h"
//#include "image.h"
//#include "message.h"
//#include "server.h"
#include "monitor.h" #include "camera.h"
#include "robot.h" #include "img.h"
#include "image.h"
#include "message.h"
#include "server.h"
extern RT_TASK th_server; #include "messages.h"
extern RT_TASK th_sendToMon; #include "commonitor.h"
extern RT_TASK th_receiveFromMon; #include "comrobot.h"
extern RT_TASK th_openComRobot;
extern RT_TASK th_startRobot;
extern RT_TASK th_move;
extern RT_MUTEX mutex_robotStarted; #include <thread>
extern RT_MUTEX mutex_move; #include <mutex>
#include <condition_variable>
extern RT_SEM sem_barrier; class Tasks {
extern RT_SEM sem_openComRobot; public:
extern RT_SEM sem_serverOk; /**
extern RT_SEM sem_startRobot; * @brief Initialisation des structures de l'application (tâches, mutex,
* semaphore, etc.)
extern RT_QUEUE q_messageToMon; */
void Init();
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;
/** /**
* \brief Thread handling server communication. * @brief Démarrage des tâches
*/ */
void f_server(void *arg); void Run();
/** /**
* \brief Thread handling communication to monitor. * @brief Arrêt des tâches
*/ */
void f_sendToMon(void *arg); void Stop();
/** /**
* \brief Thread handling communication from monitor.
*/ */
void f_receiveFromMon(void *arg); void Join() {
threadServer->join();
threadTimer->join();
threadSendToMon->join();
}
/** /**
* \brief Thread handling opening of robot communication.
*/ */
void f_openComRobot(void * arg); bool AcceptClient() {
return monitor.AcceptClient();
}
/** /**
* \brief Thread handling robot mouvements. * @brief Thread handling server communication.
*/ */
void f_move(void *arg); void ServerTask(void *arg);
/** /**
* \brief Thread handling robot activation. * @brief Thread handling server communication.
*/ */
void f_startRobot(void *arg); 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 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 // __WITH_PTHREAD__
#endif /* __TASKS_H__ */ #endif /* __TASKS_H__ */

2
software/robot/src/cmdManager.c
View file

@ -340,7 +340,7 @@ void cmdStartWithoutWatchdogAction(void) {
* Le type de commande à envoyer est :"M=val\r". Ou val * Le type de commande à envoyer est :"M=val\r". Ou val
* peut être positif ou negatif. * peut être positif ou negatif.
* *
* @param None * @param NSTART_WITH_WDone
* @retval None * @retval None
*/ */
void cmdMoveAction(void) { void cmdMoveAction(void) {