alartigu/Temps_Reel_Robot
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

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.

2
software/raspberry/superviseur-robot/lib/camera.cpp
View file

@ -1,4 +1,4 @@
/*
/*
* Copyright (C) 2018 dimercur
*
* This program is free software: you can redistribute it and/or modify

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

@ -33,25 +33,25 @@
*/
const string LABEL_MONITOR_ANSWER_ACK = "AACK";
const string LABEL_MONITOR_ANSWER_NACK = "ANAK";
const string LABEL_MONITOR_ANSWER_LOST_DMB= "ATIM";
const string LABEL_MONITOR_ANSWER_TIMEOUT= "ATIM";
const string LABEL_MONITOR_ANSWER_CMD_REJECTED= "ACRJ";
const string LABEL_MONITOR_ANSWER_COM_ERROR = "ACER";
const string LABEL_MONITOR_ANSWER_TIMEOUT = "ATIM";
const string LABEL_MONITOR_ANSWER_CMD_REJECTED = "ACRJ";
const string LABEL_MONITOR_MESSAGE = "MSSG";
const string LABEL_MONITOR_CAMERA_OPEN= "COPN";
const string LABEL_MONITOR_CAMERA_CLOSE= "CCLS";
const string LABEL_MONITOR_CAMERA_OPEN = "COPN";
const string LABEL_MONITOR_CAMERA_CLOSE = "CCLS";
const string LABEL_MONITOR_CAMERA_IMAGE = "CIMG";
const string LABEL_MONITOR_CAMERA_ARENA_ASK = "CASA";
const string LABEL_MONITOR_CAMERA_ARENA_INFIRME = "CAIN";
const string LABEL_MONITOR_CAMERA_ARENA_CONFIRM = "CACO";
const string LABEL_MONITOR_CAMERA_POSITION_COMPUTE= "CPCO";
const string LABEL_MONITOR_CAMERA_POSITION_STOP= "CPST";
const string LABEL_MONITOR_CAMERA_POSITION_COMPUTE = "CPCO";
const string LABEL_MONITOR_CAMERA_POSITION_STOP = "CPST";
const string LABEL_MONITOR_CAMERA_POSITION = "CPOS";
const string LABEL_MONITOR_ROBOT_COM_OPEN = "ROPN";
const string LABEL_MONITOR_ROBOT_COM_CLOSE = "RCLS";
const string LABEL_MONITOR_ROBOT_PING = "RPIN";
const string LABEL_MONITOR_ROBOT_RESET = "RRST";
const string LABEL_MONITOR_ROBOT_START_WITHOUT_WD= "RSOW";
const string LABEL_MONITOR_ROBOT_START_WITH_WD= "RSWW";
const string LABEL_MONITOR_ROBOT_START_WITHOUT_WD = "RSOW";
const string LABEL_MONITOR_ROBOT_START_WITH_WD = "RSWW";
const string LABEL_MONITOR_ROBOT_RELOAD_WD = "RLDW";
const string LABEL_MONITOR_ROBOT_MOVE = "RMOV";
const string LABEL_MONITOR_ROBOT_TURN = "RTRN";
@ -80,7 +80,7 @@ int ComMonitor::Open(int port) {
socketFD = socket(AF_INET, SOCK_STREAM, 0);
if (socketFD < 0) {
throw std::runtime_error{"ComMonitor::Open : Can not create socket"};
throw std::runtime_error{"Can not create socket"};
}
server.sin_addr.s_addr = INADDR_ANY;
@ -88,7 +88,8 @@ int ComMonitor::Open(int port) {
server.sin_port = htons(port);
if (bind(socketFD, (struct sockaddr *) &server, sizeof (server)) < 0) {
throw std::runtime_error{"ComMonitor::Open : Can not bind socket on port " + std::to_string(port)};
cerr<<"["<<__PRETTY_FUNCTION__<<"] Can not bind socket ("<<to_string(port)<<")"<<endl<<flush;
throw std::runtime_error{"Can not bind socket"};
}
listen(socketFD, 1);
@ -117,9 +118,7 @@ int ComMonitor::AcceptClient() {
clientID = accept(socketFD, (struct sockaddr *) &client, (socklen_t*) & c);
if (clientID < 0)
throw std::runtime_error {
"ComMonitor::AcceptClient : Accept failed"
};
throw std::runtime_error {"Accept failed"};
return clientID;
}
@ -144,7 +143,7 @@ void ComMonitor::Write(Message &msg) {
write(clientID, str.c_str(), str.length());
delete(&msg);
// Call user method after write
Write_Post();
}
@ -160,9 +159,9 @@ Message *ComMonitor::Read() {
char length = 0;
string s;
char data;
bool endReception=false;
bool endReception = false;
Message *msg;
// Call user method before read
Read_Pre();
@ -170,20 +169,20 @@ Message *ComMonitor::Read() {
while (!endReception) {
if ((length = recv(clientID, (void*) &data, 1, MSG_WAITALL)) > 0) {
if (data != '\n') {
s+=data;
s += data;
} else endReception = true;
}
}
if (length<=0) msg = new Message(MESSAGE_MONITOR_LOST);
if (length <= 0) msg = new Message(MESSAGE_MONITOR_LOST);
else {
msg=StringToMessage(s);
msg = StringToMessage(s);
}
}
// Call user method after read
Read_Post();
return msg;
}
@ -197,62 +196,56 @@ string ComMonitor::MessageToString(Message &msg) {
string str;
Message *localMsg = &msg;
Position pos;
id = msg.GetID();
switch (id) {
case MESSAGE_ANSWER:
switch (((MessageAnswer*)localMsg)->GetAnswer()) {
case ANSWER_ACK:
str.append(LABEL_MONITOR_ANSWER_ACK);
break;
case ANSWER_NACK:
str.append(LABEL_MONITOR_ANSWER_NACK);
break;
case ANSWER_LOST_ROBOT:
str.append(LABEL_MONITOR_ANSWER_LOST_DMB);
break;
case ANSWER_ROBOT_TIMEOUT:
str.append(LABEL_MONITOR_ANSWER_TIMEOUT);
break;
case ANSWER_ROBOT_UNKNOWN_COMMAND:
str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED);
break;
case ANSWER_ROBOT_ERROR:
str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED);
break;
default:
str.append(LABEL_MONITOR_ANSWER_NACK);
};
case MESSAGE_ANSWER_ACK :
str.append(LABEL_MONITOR_ANSWER_ACK);
break;
case MESSAGE_POSITION:
pos = ((MessagePosition*)&msg)->GetPosition();
case MESSAGE_ANSWER_NACK:
str.append(LABEL_MONITOR_ANSWER_NACK);
break;
case MESSAGE_ANSWER_ROBOT_TIMEOUT:
str.append(LABEL_MONITOR_ANSWER_TIMEOUT);
break;
case MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND:
str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED);
break;
case MESSAGE_ANSWER_ROBOT_ERROR:
str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED);
break;
case MESSAGE_ANSWER_COM_ERROR:
str.append(LABEL_MONITOR_ANSWER_COM_ERROR);
break;
case MESSAGE_CAM_POSITION:
pos = ((MessagePosition*) & msg)->GetPosition();
str.append(LABEL_MONITOR_CAMERA_POSITION + LABEL_SEPARATOR_CHAR + to_string(pos.robotId) + ";" +
to_string(pos.angle) + ";" + to_string(pos.center.x) + ";" + to_string(pos.center.y) + ";" +
to_string(pos.direction.x) + ";" + to_string(pos.direction.y));
break;
case MESSAGE_IMAGE:
str.append(LABEL_MONITOR_CAMERA_IMAGE + LABEL_SEPARATOR_CHAR + ((MessageImg*) &msg)->GetImage()->ToBase64());
case MESSAGE_CAM_IMAGE:
str.append(LABEL_MONITOR_CAMERA_IMAGE + LABEL_SEPARATOR_CHAR + ((MessageImg*) & msg)->GetImage()->ToBase64());
break;
case MESSAGE_ROBOT_BATTERY_LEVEL:
str.append(LABEL_MONITOR_ROBOT_BATTERY_LEVEL + LABEL_SEPARATOR_CHAR + to_string(((MessageBattery*) &msg)->GetLevel()));
str.append(LABEL_MONITOR_ROBOT_BATTERY_LEVEL + LABEL_SEPARATOR_CHAR + to_string(((MessageBattery*) & msg)->GetLevel()));
break;
case MESSAGE_ROBOT_CURRENT_STATE:
str.append(LABEL_MONITOR_ROBOT_CURRENT_STATE + LABEL_SEPARATOR_CHAR + to_string(((MessageState*) &msg)->GetState()));
case MESSAGE_ROBOT_STATE_BUSY:
str.append(LABEL_MONITOR_ROBOT_CURRENT_STATE + LABEL_SEPARATOR_CHAR + "1");
break;
case MESSAGE_ROBOT_STATE_NOT_BUSY:
str.append(LABEL_MONITOR_ROBOT_CURRENT_STATE + LABEL_SEPARATOR_CHAR + "0");
break;
case MESSAGE_LOG:
str.append(LABEL_MONITOR_MESSAGE + LABEL_SEPARATOR_CHAR + ((MessageString*) &msg)->GetString());
str.append(LABEL_MONITOR_MESSAGE + LABEL_SEPARATOR_CHAR + ((MessageString*) & msg)->GetString());
break;
case MESSAGE_EMPTY:
str.append(""); //empty string
break;
default:
throw std::runtime_error
{
"ComMonitor::MessageToString (from ComMonitor::Write): Invalid message to send (" + msg.ToString()
};
cerr<<"["<<__PRETTY_FUNCTION__<<"] (from ComMonitor::Write): Invalid message to send ("<<msg.ToString()<<")"<<endl<<flush;
throw std::runtime_error {"Invalid message to send"};
}
str.append("\n");
@ -268,69 +261,69 @@ string ComMonitor::MessageToString(Message &msg) {
Message *ComMonitor::StringToMessage(string &s) {
Message *msg;
size_t pos;
string org =s;
string org = s;
string tokenCmd;
string tokenData;
/* Separate command from data if string contains a ':' */
if ((pos=org.find(LABEL_SEPARATOR_CHAR)) != string::npos) {
tokenCmd = org.substr(0,pos);
org.erase(0,pos+1);
tokenData=org;
} else tokenCmd=org;
if ((pos = org.find(LABEL_SEPARATOR_CHAR)) != string::npos) {
tokenCmd = org.substr(0, pos);
org.erase(0, pos + 1);
tokenData = org;
} else tokenCmd = org;
/* Convert command to message */
if (tokenCmd.find(LABEL_MONITOR_ROBOT_MOVE)!= string::npos) {
msg = new MessageInt(MESSAGE_ROBOT_MOVE,stoi(tokenData));
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_TURN)!= string::npos) {
msg = new MessageInt(MESSAGE_ROBOT_TURN,stoi(tokenData));
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_START_WITHOUT_WD)!= string::npos) {
if (tokenCmd.find(LABEL_MONITOR_ROBOT_MOVE) != string::npos) {
msg = new MessageInt(MESSAGE_ROBOT_MOVE, stoi(tokenData));
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_TURN) != string::npos) {
msg = new MessageInt(MESSAGE_ROBOT_TURN, stoi(tokenData));
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_START_WITHOUT_WD) != string::npos) {
msg = new Message(MESSAGE_ROBOT_START_WITHOUT_WD);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_START_WITH_WD)!= string::npos) {
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_START_WITH_WD) != string::npos) {
msg = new Message(MESSAGE_ROBOT_START_WITH_WD);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_RELOAD_WD)!= string::npos) {
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_RELOAD_WD) != string::npos) {
msg = new Message(MESSAGE_ROBOT_RELOAD_WD);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_PING)!= string::npos) {
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_PING) != string::npos) {
msg = new Message(MESSAGE_ROBOT_PING);
} 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);
} else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_ASK)!= string::npos) {
msg = new Message(MESSAGE_ASK_ARENA);
} else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_CONFIRM)!= string::npos) {
msg = new Message(MESSAGE_ARENA_CONFIRM);
} else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_INFIRME)!= string::npos) {
msg = new Message(MESSAGE_ARENA_INFIRM);
} else if (tokenCmd.find(LABEL_MONITOR_CAMERA_CLOSE)!= string::npos) {
} else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_ASK) != string::npos) {
msg = new Message(MESSAGE_CAM_ASK_ARENA);
} else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_CONFIRM) != string::npos) {
msg = new Message(MESSAGE_CAM_ARENA_CONFIRM);
} else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_INFIRME) != string::npos) {
msg = new Message(MESSAGE_CAM_ARENA_INFIRM);
} else if (tokenCmd.find(LABEL_MONITOR_CAMERA_CLOSE) != string::npos) {
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);
} else if (tokenCmd.find(LABEL_MONITOR_CAMERA_POSITION_COMPUTE)!= string::npos) {
msg = new Message(MESSAGE_COMPUTE_POSITION);
} else if (tokenCmd.find(LABEL_MONITOR_CAMERA_POSITION_STOP)!= string::npos) {
msg = new Message(MESSAGE_STOP_COMPUTE_POSITION);
} else if (tokenCmd.find(LABEL_MONITOR_MESSAGE)!= string::npos) {
msg = new MessageString(MESSAGE_LOG,tokenData);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_COM_CLOSE)!= string::npos) {
msg = new Message(MESSAGE_CLOSE_COM);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_COM_OPEN)!= string::npos) {
msg = new Message(MESSAGE_OPEN_COM);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GET_BATTERY)!= string::npos) {
msg = new Message(MESSAGE_ROBOT_GET_BATTERY);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GET_STATE)!= string::npos) {
msg = new Message(MESSAGE_ROBOT_GET_STATE);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_FORWARD)!= string::npos) {
} else if (tokenCmd.find(LABEL_MONITOR_CAMERA_POSITION_COMPUTE) != string::npos) {
msg = new Message(MESSAGE_CAM_POSITION_COMPUTE_START);
} else if (tokenCmd.find(LABEL_MONITOR_CAMERA_POSITION_STOP) != string::npos) {
msg = new Message(MESSAGE_CAM_POSITION_COMPUTE_STOP);
} else if (tokenCmd.find(LABEL_MONITOR_MESSAGE) != string::npos) {
msg = new MessageString(MESSAGE_LOG, tokenData);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_COM_CLOSE) != string::npos) {
msg = new Message(MESSAGE_ROBOT_COM_CLOSE);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_COM_OPEN) != string::npos) {
msg = new Message(MESSAGE_ROBOT_COM_OPEN);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GET_BATTERY) != string::npos) {
msg = new Message(MESSAGE_ROBOT_BATTERY_GET);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GET_STATE) != string::npos) {
msg = new Message(MESSAGE_ROBOT_STATE_GET);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_FORWARD) != string::npos) {
msg = new Message(MESSAGE_ROBOT_GO_FORWARD);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_BACKWARD)!= string::npos) {
msg = new Message(MESSAGE_ROBOT_GO_BACK);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_LEFT)!= string::npos) {
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_BACKWARD) != string::npos) {
msg = new Message(MESSAGE_ROBOT_GO_BACKWARD);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_LEFT) != string::npos) {
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) {
msg = new Message(MESSAGE_ROBOT_GO_RIGHT);
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_POWEROFF)!= string::npos) {
} else if (tokenCmd.find(LABEL_MONITOR_ROBOT_POWEROFF) != string::npos) {
msg = new Message(MESSAGE_ROBOT_POWEROFF);
} else {
msg = new Message(MESSAGE_EMPTY);
}
return msg;
}

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

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

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

@ -35,16 +35,23 @@
/*
* Constants to be used for communicating with robot. Contains command tag
*/
typedef enum {
LABEL_ANGLE_POSITION = 'p',
LABEL_ANGULAR_SPEED = 's',
LABEL_BATTERY_LEVEL = 'b',
LABEL_BETA_ANGLE = 'v',
LABEL_USER_PRESENCE = 'u',
const char LABEL_ROBOT_PING = 'p';
const char LABEL_ROBOT_RESET = 'r';
const char LABEL_ROBOT_START_WITH_WD = 'W';
const char LABEL_ROBOT_START_WITHOUT_WD = 'u';
const char LABEL_ROBOT_RELOAD_WD = 'w';
const char LABEL_ROBOT_MOVE = 'M';
const char LABEL_ROBOT_TURN = 'T';
const char LABEL_ROBOT_GET_BATTERY = 'v';
const char LABEL_ROBOT_GET_STATE = 'b';
const char LABEL_ROBOT_POWEROFF = 'z';
LABEL_TORQUE = 'c',
LABEL_EMERGENCY_STOP = 'a'
} LabelRobot;
const char LABEL_ROBOT_OK = 'O';
const char LABEL_ROBOT_ERROR = 'E';
const char LABEL_ROBOT_UNKNOWN_COMMAND = 'C';
const char LABEL_ROBOT_SEPARATOR_CHAR = '=';
const char LABEL_ROBOT_ENDING_CHAR = 0x0D; // carriage return (\\r)
/**
* Open serial link with robot
@ -52,22 +59,35 @@ typedef enum {
* @throw std::runtime_error if it fails
*/
int ComRobot::Open() {
fd = open(USART_FILENAME, O_RDWR | O_NOCTTY /*| O_NDELAY*/); //Open in blocking read/write mode
return this->Open(USART_FILENAME);
}
/**
* Open serial link with robot
* @param usart Filename of usart to open
* @return File descriptor
* @throw std::runtime_error if it fails
*/
int ComRobot::Open(string usart) {
struct termios options;
fd = open(usart.c_str(), O_RDWR | O_NOCTTY /*| O_NDELAY*/); //Open in blocking read/write mode
if (fd == -1) {
//ERROR - CAN'T OPEN SERIAL PORT
throw std::runtime_error{"Error - Unable to open UART " + string(USART_FILENAME) + ". Ensure it is not in use by another application"};
cerr<<"["<<__PRETTY_FUNCTION__<<"] Unable to open UART ("<<usart<<"). Ensure it is not in use by another application"<<endl<<flush;
throw std::runtime_error{"Unable to open UART"};
exit(EXIT_FAILURE);
}
//Configuration of the serial port 115 520 Bauds
struct termios options;
tcgetattr(fd, &options);
options.c_cflag = B115200 | CS8 | CLOCAL | CREAD; //<Set baud rate
options.c_iflag = IGNPAR; // ignores bytes with bad parity
options.c_oflag = 0;
options.c_lflag = 0;
tcflush(fd, TCIFLUSH);
tcsetattr(fd, TCSANOW, &options);
else
{
fcntl(fd, F_SETFL, 0);
tcgetattr(fd, &options);
options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
cfsetospeed (&options, B9600);
cfsetispeed (&options, B9600);
options.c_cc[VMIN]=0;
options.c_cc[VTIME]=1; /* Timeout of 100 ms per character*/
tcsetattr(fd, TCSANOW, &options);
}
return fd;
}
@ -79,185 +99,6 @@ int ComRobot::Open() {
int ComRobot::Close() {
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
* @param msg Message to send to robot
@ -266,29 +107,142 @@ bool ComRobot::CharToBool(unsigned char *bytes) {
* @attention Write is blocking until message is written into buffer (linux side)
* @warning Write is not thread save : check that multiple tasks can't access this method simultaneously
*/
int ComRobot::Write(Message* msg) {
unsigned char buffer[7];
int ret_val = 0;
MessageToChar(msg, buffer);
Write_Pre();
Message *ComRobot::Write(Message* msg) {
Message *msgAnswer;
string s;
if (this->fd != -1) {
int count = write(this->fd, &buffer[0], 7); //Filestream, bytes to write, number of bytes to write
Write_Pre();
s=MessageToString(msg);
AddChecksum(s);
//cout << "[" <<__PRETTY_FUNCTION__<<"] Send command: "<<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) {
printf("Warning: UART TX error in ComStm32::Write\n");
} else {
ret_val = 1;
cerr << "[" << __PRETTY_FUNCTION__ << "] UART TX error (" << to_string(count) << ")" << endl << flush;
msgAnswer = new Message(MESSAGE_ANSWER_COM_ERROR);
} else { /* write successfull, read answer from robot */
try {
s = Read();
cout << "Answer = "<<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 {
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
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 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;
int val_i;
unsigned char *b;
buffer[0] = '<';
buffer[6] = '\n';
switch (msg->GetID()) {
case MESSAGE_TORQUE:
buffer[1] = LABEL_TORQUE;
val_f = ((MessageFloat*) msg)->GetValue();
b = (unsigned char *) &val_f;
case MESSAGE_ROBOT_PING:
s+=LABEL_ROBOT_PING;
break;
case MESSAGE_EMERGENCY_STOP:
buffer[1] = LABEL_EMERGENCY_STOP;
if (((MessageBool*) msg)->GetState())
val_i = 1;
else
val_i = 0;
b = (unsigned char *) &val_i;
case MESSAGE_ROBOT_RESET:
s+=LABEL_ROBOT_RESET;
break;
case MESSAGE_ROBOT_POWEROFF:
s+=LABEL_ROBOT_POWEROFF;
break;
case MESSAGE_ROBOT_START_WITHOUT_WD:
s+=LABEL_ROBOT_START_WITHOUT_WD;
break;
case MESSAGE_ROBOT_START_WITH_WD:
s+=LABEL_ROBOT_START_WITH_WD;
break;
case MESSAGE_ROBOT_RELOAD_WD:
s+=LABEL_ROBOT_RELOAD_WD;
break;
case MESSAGE_ROBOT_BATTERY_GET:
s+=LABEL_ROBOT_GET_BATTERY;
break;
case MESSAGE_ROBOT_STATE_GET:
s+=LABEL_ROBOT_GET_STATE;
break;
case MESSAGE_ROBOT_GO_FORWARD:
s+=LABEL_ROBOT_MOVE;
s+=LABEL_ROBOT_SEPARATOR_CHAR;
s.append(to_string(500000));
break;
case MESSAGE_ROBOT_GO_BACKWARD:
s+=LABEL_ROBOT_MOVE;
s+=LABEL_ROBOT_SEPARATOR_CHAR;
s.append(to_string(-500000));
break;
case MESSAGE_ROBOT_GO_LEFT:
s+=LABEL_ROBOT_TURN;
s+=LABEL_ROBOT_SEPARATOR_CHAR;
s.append(to_string(90));
break;
case MESSAGE_ROBOT_GO_RIGHT:
s+=LABEL_ROBOT_TURN;
s+=LABEL_ROBOT_SEPARATOR_CHAR;
s.append(to_string(-90));
break;
case MESSAGE_ROBOT_MOVE:
s+=LABEL_ROBOT_MOVE;
s+=LABEL_ROBOT_SEPARATOR_CHAR;
s.append(to_string(((MessageInt*)msg)->GetValue()));
break;
case MESSAGE_ROBOT_TURN:
s+=LABEL_ROBOT_TURN;
s+=LABEL_ROBOT_SEPARATOR_CHAR;
s.append(to_string(((MessageInt*)msg)->GetValue()));
break;
default:
printf("Invalid message to send");
val_i = 0;
b = (unsigned char *) &val_i;
cerr<<"["<<__PRETTY_FUNCTION__<<"] Invalid message for robot ("<<msg->ToString()<<")"<<endl<<flush;
throw std::runtime_error {"Invalid message"};
}
buffer[2] = b[0];
buffer[3] = b[1];
buffer[4] = b[2];
buffer[5] = b[3];
return s;
}
/**
* Add a checksum and carriage return to a command string
* @param[in,out] s String containing command for robot, without ending char (carriage return)
*/
void ComRobot::AddChecksum(string &s) {
unsigned char checksum=0;
for (string::iterator it=s.begin(); it!=s.end(); ++it) {
checksum ^= (unsigned char)*it;
}
s+=(char)checksum; // Add calculated checksum
s+=(char)LABEL_ROBOT_ENDING_CHAR;
}
/**
* Verify if checksum of an incoming answer from robot is valid,
* then remove checksum from incoming answer (if checksum is ok)
* @param[in,out] s String containing incoming answer from robot
* @return true is checksum is valid, false otherwise.
*/
bool ComRobot::VerifyChecksum(string &s) {
unsigned char checksum=0;
for (string::iterator it=s.begin(); it!=s.end(); ++it) {
checksum ^= (unsigned char)*it;
}
if (checksum==0) { // checksum is ok, remove last char of string (checksum)
s.pop_back(); // remove last char
return true;
}
else return false;
}

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

@ -19,6 +19,7 @@
#define __COMROBOT_H__
#include "messages.h"
#include <string>
using namespace std;
@ -30,114 +31,168 @@ using namespace std;
*/
class ComRobot {
public:
/**
* Constructor
*/
ComRobot() {}
ComRobot() {
}
/**
* Destructor
*/
virtual ~ComRobot() {}
virtual ~ComRobot() {
}
/**
* Open serial link with robot
* @return File descriptor
* @throw std::runtime_error if it fails
*/
int Open();
/**
* Open serial link with robot
* @param usart Filename of usart to open
* @return File descriptor
* @throw std::runtime_error if it fails
*/
int Open(string usart);
/**
* Close serial link
* @return Success if above 0, failure if below 0
*/
int Close();
/**
* Get a message from robot
* @return Message currently received
* @attention A message object is created (new) when receiving data from robot. You MUST remember to destroy is (delete) after use
* @attention Read method is blocking until a message is received
* @warning Read is not thread safe : Do not call it in multiple tasks simultaneously
*/
Message* Read();
/**
* Send a message to robot
* @param msg Message to send to robot
* @return 1 if success, 0 otherwise
* @attention Message is destroyed (delete) after being sent. You do not need to delete it yourself
* @return A message containing either an answer (Ack/Nak/Timeout/Error) or a value (battery level, robot state) depending of the command
* @attention Input message is destroyed (delete) after being sent. You do not need to delete it yourself
* @attention Write produce an answer message. You have to dispose it (delete) when you have finished using it
* @attention Write is blocking until message is written into buffer (linux side)
* @warning Write is not thread save : check that multiple tasks can't access this method simultaneously
*/
int Write(Message* msg);
/**
* Function called at beginning of Read method
* Use it to do some synchronization (call of mutex, for example)
*/
virtual void Read_Pre() {}
/**
* Function called at end of Read method
* Use it to do some synchronization (call of mutex, for example)
*/
virtual void Read_Post() {}
Message *Write(Message* msg);
/**
* Function called at beginning of Write method
* Use it to do some synchronization (call of mutex, for example)
*/
virtual void Write_Pre() {}
virtual void Write_Pre() {
}
/**
* Function called at end of Write method
* Use it to do some synchronization (call of mutex, for example)
*/
virtual void Write_Post() {}
virtual void Write_Post() {
}
static Message *Ping();
Message *SendCommand(Message* msg, MessageID answerID, int maxRetries);
static Message *Ping() {
return new Message(MESSAGE_ROBOT_PING);
}
static Message *Reset() {
return new Message(MESSAGE_ROBOT_RESET);
}
static Message *PowerOff() {
return new Message(MESSAGE_ROBOT_POWEROFF);
}
static Message *StartWithWD() {
return new Message(MESSAGE_ROBOT_START_WITH_WD);
}
static Message *StartWithoutWD() {
return new Message(MESSAGE_ROBOT_START_WITHOUT_WD);
}
static Message *ReloadWD() {
return new Message(MESSAGE_ROBOT_RELOAD_WD);
}
static Message *Move(int length) {
return new MessageInt(MESSAGE_ROBOT_MOVE, length);
}
static Message *Turn(int angle) {
return new MessageInt(MESSAGE_ROBOT_TURN, angle);
}
static Message *Stop() {
return new Message(MESSAGE_ROBOT_STOP);
}
static Message *GoForward() {
return new Message(MESSAGE_ROBOT_GO_FORWARD);
}
static Message *GoBackward() {
return new Message(MESSAGE_ROBOT_GO_BACKWARD);
}
static Message *GoLeft() {
return new Message(MESSAGE_ROBOT_GO_LEFT);
}
static Message *GoRight() {
return new Message(MESSAGE_ROBOT_GO_RIGHT);
}
static Message *GetBattery() {
return new Message(MESSAGE_ROBOT_BATTERY_GET);
}
static Message *GetState() {
return new Message(MESSAGE_ROBOT_STATE_GET);
}
protected:
/**
* Serial link file descriptor
*/
int fd;
/**
* Convert an array of char to float
* @param bytes Array of char, containing a binary image of a float
* @return Float value
* Get an answer from robot
* @return String containing answer from robot
* @attention Read method is blocking until a message is received (timeout of 500 ms)
* @warning Read is not thread safe : Do not call it in multiple tasks simultaneously
*/
float CharToFloat(unsigned char *bytes);
string Read();
/**
* Convert an array of char to boolean
* @param bytes Array of char, containing a binary image of a boolean
* @return Boolean value
*/
bool CharToBool(unsigned char *bytes);
/**
* Convert an array of char to integer
* @param bytes Array of char, containing a binary image of an integer
* @return Integer value
*/
unsigned int CharToInt(unsigned char *bytes);
/**
* Convert an array of char to its message representation (when receiving data from stm32)
* @param bytes Array of char
* Convert a string to its message representation (when receiving data from robot)
* @param s String from robot containing answer
* @return Message corresponding to received array of char
*/
Message* CharToMessage(unsigned char *bytes);
Message* StringToMessage(string s);
/**
* Convert a message to its array of char representation (for sending command to stm32)
* Convert a message to its string representation (for sending command to robot)
* @param msg Message to be sent to robot
* @param buffer Array of char, image of message to send
* @return String containing command to robot
*/
void MessageToChar(Message *msg, unsigned char *buffer);
string MessageToString(Message *msg);
/**
* Add a checksum and carriage return to a command string
* @param[in,out] s String containing command for robot, without ending char (carriage return)
*/
void AddChecksum(string &s);
/**
* Verify if checksum of an incoming answer from robot is valid,
* then remove checksum from incoming answer (if checksum is ok)
* @param[in,out] s String containing incoming answer from robot
* @return true is checksum is valid, false otherwise.
*/
bool VerifyChecksum(string &s);
};
#endif /* __COMROBOT_H__ */

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

@ -27,9 +27,13 @@ Img::Img(ImageMat imgMatrice) {
}
string Img::ToString() {
return "Image size: "+this->img.cols+"x"this->img.rows+" (dim="+this->img.dims+")";
return "Image size: "+to_string(this->img.cols)+"x"+to_string(this->img.rows)+" (dim="+to_string(this->img.dims)+")";
}
string Img::ToBase64() {
return "";
}
Img* Img::Copy() {
return new Img(this->img);
}
@ -100,13 +104,13 @@ Jpg Img::toJpg() {
return imgJpg;
}
string Img::ToBase64() {
string imgBase64;
Jpg imgJpg = toJpg();
/* faire la convertion Jpg vers base 64 */
return imgBase64;
}
//string Img::ToBase64() {
// string imgBase64;
// Jpg imgJpg = toJpg();
//
// /* faire la convertion Jpg vers base 64 */
// return imgBase64;
//}
std::list<Position> Img::search_robot(Arene monArene) {

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

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

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

@ -27,7 +27,12 @@
const string MESSAGE_ID_STRING[] = {
"Empty",
"Log",
"Answer",
"Answer [Acknowledge]",
"Answer [Not Acknowledge]",
"Answer [Command timeout]",
"Answer [Command unknown]",
"Answer [Command error]",
"Answer [Communication error]",
"Monitor connection lost",
"Open serial com",
"Close serial com",
@ -42,8 +47,8 @@ const string MESSAGE_ID_STRING[] = {
"Image",
"Robot ping",
"Robot reset",
"Robot start with wtachdog",
"Robot start without wtachdog",
"Robot start with watchdog",
"Robot start without watchdog",
"Robot reload watchdog",
"Robot move",
"Robot turn",
@ -56,7 +61,9 @@ const string MESSAGE_ID_STRING[] = {
"Robot get battery",
"Robot battery level",
"Robot get state",
"Robot current state"
"Robot current state",
"Robot state [Not busy]",
"Robot state [Busy]"
};
/*
@ -108,7 +115,7 @@ void Message::SetID(MessageID id) {
*/
string Message::ToString() {
if (CheckID(this->messageID))
return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"";
return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"";
else
return "Invalid message";
}
@ -118,7 +125,7 @@ string Message::ToString() {
* @return A message, copy of current
*/
Message* Message::Copy() {
Message *msg = new Message();
Message *msg = new Message(this->messageID);
return msg;
}
@ -128,30 +135,12 @@ Message* Message::Copy() {
* @return Current message ID
*/
bool Message::CheckID(MessageID id) {
if ((id != MESSAGE_EMPTY) &&
(id != MESSAGE_MONITOR_LOST) &&
(id != MESSAGE_ARENA_CONFIRM) &&
(id != MESSAGE_ARENA_INFIRM) &&
(id != MESSAGE_ASK_ARENA) &&
(id != MESSAGE_CAM_CLOSE) &&
(id != MESSAGE_CAM_OPEN) &&
(id != MESSAGE_CLOSE_COM) &&
(id != MESSAGE_COMPUTE_POSITION) &&
(id != MESSAGE_OPEN_COM) &&
(id != MESSAGE_ROBOT_GET_BATTERY) &&
(id != MESSAGE_ROBOT_GET_STATE) &&
(id != MESSAGE_ROBOT_GO_BACK) &&
(id != MESSAGE_ROBOT_GO_FORWARD) &&
(id != MESSAGE_ROBOT_GO_LEFT) &&
(id != MESSAGE_ROBOT_GO_RIGHT) &&
(id != MESSAGE_ROBOT_PING) &&
(id != MESSAGE_ROBOT_POWEROFF) &&
(id != MESSAGE_ROBOT_RELOAD_WD) &&
(id != MESSAGE_ROBOT_RESET) &&
(id != MESSAGE_ROBOT_START_WITHOUT_WD) &&
(id != MESSAGE_ROBOT_START_WITH_WD) &&
(id != MESSAGE_ROBOT_STOP) &&
(id != MESSAGE_STOP_COMPUTE_POSITION)) {
if ((id == MESSAGE_CAM_IMAGE) ||
(id == MESSAGE_CAM_POSITION) ||
(id == MESSAGE_ROBOT_MOVE) ||
(id == MESSAGE_ROBOT_TURN) ||
(id == MESSAGE_LOG) ||
(id == MESSAGE_ROBOT_BATTERY_LEVEL)) {
return false;
} else return true;
}
@ -197,7 +186,7 @@ void MessageInt::SetID(MessageID id) {
*/
string MessageInt::ToString() {
if (CheckID(this->messageID))
return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nValue: " + to_string(this->value);
return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nValue: " + to_string(this->value);
else
return "Invalid message";
}
@ -263,7 +252,7 @@ void MessageString::SetID(MessageID id) {
*/
string MessageString::ToString() {
if (CheckID(this->messageID))
return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nString: \"" + this->s + "\"";
return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nString: \"" + this->s + "\"";
else
return "Invalid message";
}
@ -342,7 +331,7 @@ void MessageImg::SetImage(Img* image) {
*/
string MessageImg::ToString() {
if (CheckID(this->messageID))
return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\n" + this->image->ToString();
return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\n" + this->image->ToString();
else
return "Invalid message";
}
@ -362,91 +351,7 @@ Message* MessageImg::Copy() {
* @return true, if message ID is acceptable, false otherwise
*/
bool MessageImg::CheckID(MessageID id) {
if (id != MESSAGE_IMAGE) {
return false;
} else return true;
}
/* class MessageAnswer*/
/**
* Create a new, empty answer message
*/
MessageAnswer::MessageAnswer() {
answer=ANSWER_ACK;
}
/**
* Create a new answer message, with given ID and answer
* @param id Message ID
* @param ans Answer ID
* @throw std::runtime_error if message ID is incompatible with string data
*/
MessageAnswer::MessageAnswer(MessageID id, AnswerID ans) {
MessageAnswer::SetID(id);
MessageAnswer::SetAnswer(ans);
}
/**
* Set message ID
* @param id Message ID
* @throw std::runtime_error if message ID is incompatible with answer message
*/
void MessageAnswer::SetID(MessageID id) {
if (CheckID(id))
messageID = id;
else
throw std::runtime_error {
"Invalid message id for MessageAnswer"
};
}
/**
* Set message answer
* @param ans Answer ID
* @throw std::runtime_error if answer ID is incompatible with answer data
*/
void MessageAnswer::SetAnswer(AnswerID ans) {
if ((ans != ANSWER_ACK) &&
(ans != ANSWER_NACK) &&
(ans != ANSWER_LOST_ROBOT) &&
(ans != ANSWER_ROBOT_CHECKSUM) &&
(ans != ANSWER_ROBOT_ERROR) &&
(ans != ANSWER_ROBOT_TIMEOUT) &&
(ans != ANSWER_ROBOT_UNKNOWN_COMMAND)) {
this->answer = answer;
} else {
throw std::runtime_error{
"Invalid answer for MessageAnswer"};
}
}
/**
* Translate content of message into a string that can be displayed
* @return A string describing message contents
*/
string MessageAnswer::ToString() {
if (CheckID(this->messageID))
return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nAnswer: \"" + ANSWER_ID_STRING[this->answer] + "\"";
else
return "Invalid message";
}
/**
* Allocate a new message and copy contents of current message
* @return A message, copy of current
*/
Message* MessageAnswer::Copy() {
return new MessageAnswer(this->messageID, this->answer);
}
/**
* Verify if message ID is compatible with current message type
* @param id Message ID
* @return true, if message ID is acceptable, false otherwise
*/
bool MessageAnswer::CheckID(MessageID id) {
if ((id != MESSAGE_ANSWER)) {
if (id != MESSAGE_CAM_IMAGE) {
return false;
} else return true;
}
@ -523,7 +428,7 @@ string MessageBattery::ToString() {
}
if (CheckID(this->messageID))
return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nBattery level: \"" + levelString + "\"";
return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nBattery level: \"" + levelString + "\"";
else
return "Invalid message";
}
@ -603,7 +508,7 @@ void MessagePosition::SetPosition(Position& pos) {
*/
string MessagePosition::ToString() {
if (CheckID(this->messageID))
return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nPosition: \"" + to_string(this->pos.center.x) + ";" + to_string(this->pos.center.y) + "\"";
return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nPosition: \"" + to_string(this->pos.center.x) + ";" + to_string(this->pos.center.y) + "\"";
else
return "Invalid message";
}
@ -622,91 +527,7 @@ Message* MessagePosition::Copy() {
* @return true, if message ID is acceptable, false otherwise
*/
bool MessagePosition::CheckID(MessageID id) {
if ((id != MESSAGE_POSITION)) {
if ((id != MESSAGE_CAM_POSITION)) {
return false;
} else return true;
}
/* class MessageState */
/**
* Create a new, empty state message
*/
MessageState::MessageState() {
state = ROBOT_NOT_BUSY;
}
/**
* Create a new string message, with given ID and string
* @param id Message ID
* @param s Message string
* @throw std::runtime_error if message ID is incompatible with string data
*/
MessageState::MessageState(MessageID id, RobotState state) {
MessageState::SetID(id);
MessageState::SetState(state);
}
/**
* Set message ID
* @param id Message ID
* @throw std::runtime_error if message ID is incompatible with robot state
*/
void MessageState::SetID(MessageID id) {
if (CheckID(id))
messageID = id;
else
throw std::runtime_error {
"Invalid message id for MessageState"
};
}
/**
* Set robot state
* @param state Robot state
*/
void MessageState::SetState(RobotState state) {
if ((state != ROBOT_NOT_BUSY) && (state != ROBOT_BUSY)) {
throw std::runtime_error{
"Invalid state for MessageState"};
} else {
this->state = state;
}
}
/**
* Translate content of message into a string that can be displayed
* @return A string describing message contents
*/
string MessageState::ToString() {
string stateString;
if (this->state == ROBOT_NOT_BUSY) stateString="Not busy";
else if (this->state == ROBOT_BUSY) stateString="Busy";
else stateString="Invalid state";
if (CheckID(this->messageID))
return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nState: \"" + stateString + "\"";
else
return "Invalid message";
}
/**
* Allocate a new message and copy contents of current message
* @return A message, copy of current
*/
Message* MessageState::Copy() {
return new MessageState(this->messageID, this->state);
}
/**
* Verify if message ID is compatible with current message type
* @param id Message ID
* @return true, if message ID is acceptable, false otherwise
*/
bool MessageState::CheckID(MessageID id) {
if ((id != MESSAGE_ROBOT_CURRENT_STATE)) {
return false;
} else return true;
}

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

@ -33,11 +33,16 @@ typedef enum {
MESSAGE_LOG,
// Message containing answer (after robot command, or for monitor)
MESSAGE_ANSWER,
MESSAGE_ANSWER_ACK,
MESSAGE_ANSWER_NACK,
MESSAGE_ANSWER_ROBOT_TIMEOUT,
MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND,
MESSAGE_ANSWER_ROBOT_ERROR,
MESSAGE_ANSWER_COM_ERROR,
// messages for serial communication with robot
MESSAGE_OPEN_COM,
MESSAGE_CLOSE_COM,
MESSAGE_ROBOT_COM_OPEN,
MESSAGE_ROBOT_COM_CLOSE,
// Messages specific to server
MESSAGE_MONITOR_LOST,
@ -45,13 +50,13 @@ typedef enum {
// Messages for camera
MESSAGE_CAM_OPEN,
MESSAGE_CAM_CLOSE,
MESSAGE_ASK_ARENA,
MESSAGE_ARENA_CONFIRM,
MESSAGE_ARENA_INFIRM,
MESSAGE_COMPUTE_POSITION,
MESSAGE_STOP_COMPUTE_POSITION,
MESSAGE_POSITION,
MESSAGE_IMAGE,
MESSAGE_CAM_ASK_ARENA,
MESSAGE_CAM_ARENA_CONFIRM,
MESSAGE_CAM_ARENA_INFIRM,
MESSAGE_CAM_POSITION_COMPUTE_START,
MESSAGE_CAM_POSITION_COMPUTE_STOP,
MESSAGE_CAM_POSITION,
MESSAGE_CAM_IMAGE,
// Messages for robot
MESSAGE_ROBOT_PING,
@ -62,27 +67,18 @@ typedef enum {
MESSAGE_ROBOT_MOVE,
MESSAGE_ROBOT_TURN,
MESSAGE_ROBOT_GO_FORWARD,
MESSAGE_ROBOT_GO_BACK,
MESSAGE_ROBOT_GO_BACKWARD,
MESSAGE_ROBOT_GO_LEFT,
MESSAGE_ROBOT_GO_RIGHT,
MESSAGE_ROBOT_STOP,
MESSAGE_ROBOT_POWEROFF,
MESSAGE_ROBOT_GET_BATTERY,
MESSAGE_ROBOT_BATTERY_GET,
MESSAGE_ROBOT_BATTERY_LEVEL,
MESSAGE_ROBOT_GET_STATE,
MESSAGE_ROBOT_CURRENT_STATE
MESSAGE_ROBOT_STATE_GET,
MESSAGE_ROBOT_STATE_NOT_BUSY,
MESSAGE_ROBOT_STATE_BUSY
} MessageID;
typedef enum {
ANSWER_ACK=0,
ANSWER_NACK,
ANSWER_LOST_ROBOT,
ANSWER_ROBOT_TIMEOUT,
ANSWER_ROBOT_UNKNOWN_COMMAND,
ANSWER_ROBOT_ERROR,
ANSWER_ROBOT_CHECKSUM
} AnswerID;
typedef enum {
BATTERY_UNKNOWN=-1,
BATTERY_EMPTY=0,
@ -90,11 +86,6 @@ typedef enum {
BATTERY_FULL
} BatteryLevel;
typedef enum {
ROBOT_NOT_BUSY=0,
ROBOT_BUSY
} RobotState;
using namespace std;
/**
@ -132,6 +123,15 @@ public:
*/
virtual Message* Copy();
/**
* Compare message ID
* @param id Id to compare message to
* @return true if id is equal to message id, false otherwise
*/
bool CompareID(MessageID id) {
return (this->messageID == id) ? true:false;
}
/**
* Get message ID
* @return Current message ID
@ -563,140 +563,5 @@ protected:
bool CheckID(MessageID id);
};
/**
* Message class for holding answer, based on Message class
*
* @brief Answer message class
*
*/
class MessageAnswer : public Message {
public:
/**
* Create a new, empty image message
*/
MessageAnswer();
/**
* Create a new image message, with given ID and boolean value
* @param id Message ID
* @param ans Answer ID
* @throw std::runtime_error if message ID is incompatible with image message
*/
MessageAnswer(MessageID id, AnswerID ans);
/**
* Set message ID
* @param id Message ID
* @throw std::runtime_error if message ID is incompatible withimage message
*/
void SetID(MessageID id);
/**
* Get message image
* @return Pointer to image
*/
AnswerID GetAnswer() {
return answer;
}
/**
* Set message answer
* @param ans Answer ID
*/
void SetAnswer(AnswerID ans);
/**
* Translate content of message into a string that can be displayed
* @return A string describing message contents
*/
string ToString();
/**
* Allocate a new message and copy contents of current message
* @return A message, copy of current
*/
Message* Copy();
protected:
/**
* Message answer
*/
AnswerID answer;
/**
* Verify if message ID is compatible with current message type
* @param id Message ID
* @return true, if message ID is acceptable, false otherwise
*/
bool CheckID(MessageID id);
};
/**
* Message class for holding robot state, based on Message class
*
* @brief Answer message class
*
*/
class MessageState: public Message {
public:
/**
* Create a new, empty image message
*/
MessageState();
/**
* Create a new image message, with given ID and boolean value
* @param id Message ID
* @param image Pointer to image
* @throw std::runtime_error if message ID is incompatible with image message
*/
MessageState(MessageID id, RobotState state);
/**
* Set message ID
* @param id Message ID
* @throw std::runtime_error if message ID is incompatible withimage message
*/
void SetID(MessageID id);
/**
* Get message image
* @return Pointer to image
*/
RobotState GetState() {
return state;
}
/**
* Set message image
* @param image Pointer to image object
*/
void SetState(RobotState state);
/**
* Translate content of message into a string that can be displayed
* @return A string describing message contents
*/
string ToString();
/**
* Allocate a new message and copy contents of current message
* @return A message, copy of current
*/
Message* Copy();
protected:
/**
* Robot state
*/
RobotState state;
/**
* Verify if message ID is compatible with current message type
* @param id Message ID
* @return true, if message ID is acceptable, false otherwise
*/
bool CheckID(MessageID id);
};
#endif /* __MESSAGES_H__ */

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/>.
*/
/**
* \file main.cpp
* \author PE.Hladik
* \version 1.0
* \date 06/06/2017
* \brief main program
*/
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <unistd.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"
// Déclaration des taches
RT_TASK th_server;
RT_TASK th_sendToMon;
RT_TASK th_receiveFromMon;
RT_TASK th_openComRobot;
RT_TASK th_startRobot;
RT_TASK th_move;
// Déclaration des priorités des taches
int PRIORITY_TSERVER = 30;
int PRIORITY_TOPENCOMROBOT = 20;
int PRIORITY_TMOVE = 10;
int PRIORITY_TSENDTOMON = 25;
int PRIORITY_TRECEIVEFROMMON = 22;
int PRIORITY_TSTARTROBOT = 20;
RT_MUTEX mutex_robotStarted;
RT_MUTEX mutex_move;
// Déclaration des sémaphores
RT_SEM sem_barrier;
RT_SEM sem_openComRobot;
RT_SEM sem_serverOk;
RT_SEM sem_startRobot;
// Déclaration des files de message
RT_QUEUE q_messageToMon;
int MSG_QUEUE_SIZE = 10;
// Déclaration des ressources partagées
int etatCommMoniteur = 1;
int robotStarted = 0;
char robotMove = DMB_STOP_MOVE;
/**
* \fn void initStruct(void)
* \brief Initialisation des structures de l'application (tâches, mutex,
* semaphore, etc.)
*/
void initStruct(void);
/**
* \fn void startTasks(void)
* \brief Démarrage des tâches
*/
void startTasks(void);
/**
* \fn void deleteTasks(void)
* \brief Arrêt des tâches
*/
void deleteTasks(void);
#endif // __WITH_PTHREAD__
int main(int argc, char **argv) {
int err;
Tasks tasks;
//Lock the memory to avoid memory swapping for this program
mlockall(MCL_CURRENT | MCL_FUTURE);
printf("#################################\n");
printf("# DE STIJL PROJECT #\n");
printf("#################################\n");
cout<<"#################################"<<endl;
cout<<"# DE STIJL PROJECT #"<<endl;
cout<<"#################################"<<endl;
initStruct();
startTasks();
rt_sem_broadcast(&sem_barrier);
pause();
deleteTasks();
tasks.Init();
/*if (tasks.AcceptClient()) {
tasks.Run();
tasks.Join();
}
tasks.Stop();*/
tasks.Run();
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}/main.o \
${OBJECTDIR}/tasks.o \
${OBJECTDIR}/_ext/6cc0dc4a/camera.o \
${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \
${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \
${OBJECTDIR}/_ext/6cc0dc4a/img.o \
${OBJECTDIR}/tasks_pthread.o
@ -106,6 +108,11 @@ ${OBJECTDIR}/tasks.o: tasks.cpp
${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
${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
${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
${RM} "$@.d"
@ -116,6 +123,11 @@ ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o: /home/dimercur/Documents/Travail/git/dumb
${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
${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
${MKDIR} -p ${OBJECTDIR}
${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
OBJECTFILES= \
${OBJECTDIR}/lib/message.o \
${OBJECTDIR}/lib/messages.o \
${OBJECTDIR}/lib/monitor.o \
${OBJECTDIR}/lib/robot.o \
${OBJECTDIR}/lib/server.o \
${OBJECTDIR}/main.o \
${OBJECTDIR}/_ext/6cc0dc4a/camera.o \
${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \
${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \
${OBJECTDIR}/_ext/6cc0dc4a/img.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
# CC Compiler Flags
CCFLAGS=-D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables
CXXFLAGS=-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 -Wno-pmf-conversions
# Fortran Compiler Flags
FFLAGS=
@ -70,36 +68,21 @@ ${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}/superviseur-robot: ${OBJECTFILES}
${MKDIR} -p ${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}
${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
${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/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
${MKDIR} -p ${OBJECTDIR}
${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
${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
${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
${RM} "$@.d"
@ -110,6 +93,11 @@ ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o: /home/dimercur/Documents/Travail/git/dumb
${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
${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
${MKDIR} -p ${OBJECTDIR}
${RM} "$@.d"

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

@ -43,8 +43,10 @@ OBJECTFILES= \
${OBJECTDIR}/lib/server.o \
${OBJECTDIR}/main.o \
${OBJECTDIR}/tasks.o \
${OBJECTDIR}/_ext/6cc0dc4a/camera.o \
${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \
${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \
${OBJECTDIR}/_ext/6cc0dc4a/img.o \
${OBJECTDIR}/tasks_pthread.o
@ -112,6 +114,11 @@ ${OBJECTDIR}/tasks.o: tasks.cpp
${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
${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
${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
${RM} "$@.d"
@ -122,6 +129,11 @@ ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o: /home/dimercur/Documents/Travail/git/dumb
${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
${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
${MKDIR} -p ${OBJECTDIR}
${RM} "$@.d"

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

@ -43,8 +43,10 @@ OBJECTFILES= \
${OBJECTDIR}/lib/server.o \
${OBJECTDIR}/main.o \
${OBJECTDIR}/tasks.o \
${OBJECTDIR}/_ext/6cc0dc4a/camera.o \
${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \
${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \
${OBJECTDIR}/_ext/6cc0dc4a/img.o \
${OBJECTDIR}/tasks_pthread.o
@ -112,6 +114,11 @@ ${OBJECTDIR}/tasks.o: tasks.cpp
${RM} "$@.d"
$(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
${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
${RM} "$@.d"
@ -122,6 +129,11 @@ ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o: /home/dimercur/Documents/Travail/git/dumb
${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
${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
${MKDIR} -p ${OBJECTDIR}
${RM} "$@.d"

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

@ -26,11 +26,11 @@
<logicalFolder name="SourceFiles"
displayName="Source Files"
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/comrobot.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>./lib/message.cpp</itemPath>
<itemPath>./lib/messages.cpp</itemPath>
@ -93,16 +93,12 @@
</compileType>
<item path="./gdbsudo.sh" ex="false" tool="3" flavor2="0">
</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>
<item path="./lib/definitions.h" ex="false" tool="3" flavor2="0">
</item>
<item path="./lib/image.h" ex="false" tool="3" flavor2="0">
</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>
<item path="./lib/message.cpp" ex="false" tool="1" flavor2="0">
@ -131,6 +127,11 @@
</item>
<item path="./tasks.h" ex="false" tool="3" flavor2="0">
</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"
ex="false"
tool="1"
@ -151,6 +152,11 @@
tool="3"
flavor2="0">
</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>
<item path="tasks_pthread.h" ex="false" tool="3" flavor2="0">
@ -178,8 +184,6 @@
</compileType>
<item path="./gdbsudo.sh" ex="false" tool="3" flavor2="0">
</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>
<item path="./lib/definitions.h" ex="false" tool="3" flavor2="0">
@ -188,8 +192,6 @@
</item>
<item path="./lib/image.h" ex="false" tool="3" flavor2="0">
</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>
<item path="./lib/message.cpp" ex="false" tool="1" flavor2="0">
@ -218,6 +220,11 @@
</item>
<item path="./tasks.h" ex="false" tool="3" flavor2="0">
</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"
ex="false"
tool="1"
@ -238,6 +245,11 @@
tool="3"
flavor2="0">
</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>
<item path="tasks_pthread.h" ex="false" tool="3" flavor2="0">
@ -282,8 +294,6 @@
</compileType>
<item path="./gdbsudo.sh" ex="false" tool="3" flavor2="0">
</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>
<item path="./lib/definitions.h" ex="false" tool="3" flavor2="0">
@ -292,8 +302,6 @@
</item>
<item path="./lib/image.h" ex="false" tool="3" flavor2="0">
</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>
<item path="./lib/message.cpp" ex="false" tool="1" flavor2="0">
@ -322,6 +330,11 @@
</item>
<item path="./tasks.h" ex="false" tool="3" flavor2="0">
</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"
ex="false"
tool="1"
@ -342,6 +355,11 @@
tool="3"
flavor2="0">
</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>
<item path="tasks_pthread.h" ex="false" tool="3" flavor2="0">
@ -366,7 +384,7 @@
<pElem>./</pElem>
<pElem>./lib</pElem>
</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>
<Elem>_WITH_TRACE_</Elem>
<Elem>__FOR_PC__</Elem>
@ -382,8 +400,6 @@
</compileType>
<item path="./gdbsudo.sh" ex="false" tool="3" flavor2="0">
</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>
<item path="./lib/definitions.h" ex="false" tool="3" flavor2="0">
@ -392,11 +408,9 @@
</item>
<item path="./lib/image.h" ex="false" tool="3" flavor2="0">
</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>
<item path="./lib/message.cpp" ex="false" tool="1" flavor2="9">
<item path="./lib/message.cpp" ex="true" tool="1" flavor2="9">
</item>
<item path="./lib/message.h" ex="false" tool="3" flavor2="0">
</item>
@ -404,15 +418,15 @@
</item>
<item path="./lib/messages.h" ex="false" tool="3" flavor2="0">
</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 path="./lib/monitor.h" ex="false" tool="3" flavor2="0">
</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 path="./lib/robot.h" ex="false" tool="3" flavor2="0">
</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 path="./lib/server.h" ex="false" tool="3" flavor2="0">
</item>
@ -420,6 +434,11 @@
</item>
<item path="./tasks.cpp" ex="true" tool="1" flavor2="9">
</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"
ex="false"
tool="1"
@ -440,6 +459,11 @@
tool="3"
flavor2="0">
</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>
<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>
<gdbinterceptoptions gdb_all="false" gdb_unhandled="true" gdb_unexpected="true"/>
</gdb_interceptlist>
<gdb_signals>
</gdb_signals>
<gdb_options>
<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"/>
<open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2">
<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.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/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/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/tasks_pthread.cpp</file>
</group>
</open-files>
</project-private>

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

@ -15,16 +15,18 @@
* 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"
#ifndef __WITH_PTHREAD__
// Déclaration des priorités des taches
#define PRIORITY_TSERVER 30
#define PRIORITY_TOPENCOMROBOT 20
#define PRIORITY_TMOVE 10
#define PRIORITY_TSENDTOMON 25
#define PRIORITY_TRECEIVEFROMMON 22
#define PRIORITY_TSTARTROBOT 20
char mode_start;
void write_in_queue(RT_QUEUE *, MessageToMon);
@ -258,4 +260,6 @@ void write_in_queue(RT_QUEUE *queue, MessageToMon msg) {
buff = rt_queue_alloc(&q_messageToMon, sizeof (MessageToMon));
memcpy(buff, &msg, sizeof (MessageToMon));
rt_queue_send(&q_messageToMon, buff, sizeof (MessageToMon), Q_NORMAL);
}
}
#endif // __WITH_PTHREAD__

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

@ -15,17 +15,10 @@
* 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__
#define __TASKS_H__
#ifndef __WITH_PTHREAD__
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
@ -37,71 +30,107 @@
#include <alchemy/sem.h>
#include <alchemy/queue.h>
#include "monitor.h"
#include "robot.h"
#include "image.h"
#include "message.h"
#include "server.h"
//#include "monitor.h"
//#include "robot.h"
//#include "image.h"
//#include "message.h"
//#include "server.h"
extern RT_TASK th_server;
extern RT_TASK th_sendToMon;
extern RT_TASK th_receiveFromMon;
extern RT_TASK th_openComRobot;
extern RT_TASK th_startRobot;
extern RT_TASK th_move;
#include "messages.h"
#include "commonitor.h"
#include "comrobot.h"
extern RT_MUTEX mutex_robotStarted;
extern RT_MUTEX mutex_move;
using namespace std;
extern RT_SEM sem_barrier;
extern RT_SEM sem_openComRobot;
extern RT_SEM sem_serverOk;
extern RT_SEM sem_startRobot;
class Tasks {
public:
/**
* @brief Initialisation des structures de l'application (tâches, mutex,
* semaphore, etc.)
*/
void Init();
extern RT_QUEUE q_messageToMon;
/**
* @brief Démarrage des tâches
*/
void Run();
extern int etatCommMoniteur;
extern int robotStarted;
extern char robotMove;
/**
* @brief Arrêt des tâches
*/
void Stop();
/**
*/
void Join() {
rt_sem_broadcast(&sem_barrier);
pause();
}
/**
*/
bool AcceptClient() {
return false;
}
private:
ComMonitor monitor;
ComRobot robot;
RT_TASK th_server;
RT_TASK th_sendToMon;
RT_TASK th_receiveFromMon;
RT_TASK th_openComRobot;
RT_TASK th_startRobot;
RT_TASK th_move;
extern int MSG_QUEUE_SIZE;
RT_MUTEX mutex_robotStarted;
RT_MUTEX mutex_move;
extern int PRIORITY_TSERVER;
extern int PRIORITY_TOPENCOMROBOT;
extern int PRIORITY_TMOVE;
extern int PRIORITY_TSENDTOMON;
extern int PRIORITY_TRECEIVEFROMMON;
extern int PRIORITY_TSTARTROBOT;
RT_SEM sem_barrier;
RT_SEM sem_openComRobot;
RT_SEM sem_serverOk;
RT_SEM sem_startRobot;
/**
* \brief Thread handling server communication.
*/
void f_server(void *arg);
RT_QUEUE q_messageToMon;
/**
* \brief Thread handling communication to monitor.
*/
void f_sendToMon(void *arg);
int etatCommMoniteur;
int robotStarted;
char robotMove;
/**
* \brief Thread handling communication from monitor.
*/
void f_receiveFromMon(void *arg);
int MSG_QUEUE_SIZE;
/**
* \brief Thread handling opening of robot communication.
*/
void f_openComRobot(void * arg);
/**
* \brief Thread handling server communication.
*/
void f_server(void *arg);
/**
* \brief Thread handling robot mouvements.
*/
void f_move(void *arg);
/**
* \brief Thread handling communication to monitor.
*/
void f_sendToMon(void *arg);
/**
* \brief Thread handling robot activation.
*/
void f_startRobot(void *arg);
/**
* \brief Thread handling communication from monitor.
*/
void f_receiveFromMon(void *arg);
#endif /* FUNCTIONS_H */
/**
* \brief Thread handling opening of robot communication.
*/
void f_openComRobot(void * arg);
/**
* \brief Thread handling robot mouvements.
*/
void f_move(void *arg);
/**
* \brief Thread handling robot activation.
*/
void f_startRobot(void *arg);
};
#endif // __WITH_PTHREAD__
#endif // __TASKS_H__

538
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/>.
*/
/**
* \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 <time.h>
#ifdef __WITH_PTHREAD__
char mode_start;
void write_in_queue(RT_QUEUE *, MessageToMon);
// Déclaration des priorités des taches
#define PRIORITY_TSERVER 30
#define PRIORITY_TOPENCOMROBOT 20
#define PRIORITY_TMOVE 10
#define PRIORITY_TSENDTOMON 25
#define PRIORITY_TRECEIVEFROMMON 22
#define PRIORITY_TSTARTROBOT 20
void f_server(void *arg) {
/*
* Some remarks:
* 1- This program is mostly a template. It shows you how to create tasks, semaphore
* message queues, mutex ... and how to use them
*
* 2- semDumber is, as name say, useless. Its goal is only to show you how to use semaphore
*
* 3- Data flow is probably not optimal
*
* 4- Take into account that ComRobot::Write will block your task when serial buffer is full,
* time for internal buffer to flush
*
* 5- Same behavior existe for ComMonitor::Write !
*
* 6- When you want to write something in terminal, use cout and terminate with endl and flush
*
* 7- Good luck !
*/
void Tasks::Init() {
int status;
/* Open com port with STM32 */
cout << "Open serial com (";
status = robot.Open("/dev/ttyUSB1");
cout << status;
cout << ")" << endl;
if (status >= 0) {
// Open server
status = monitor.Open(SERVER_PORT);
cout << "Open server on port " << SERVER_PORT << " (" << status << ")" << endl;
if (status < 0) throw std::runtime_error {
"Unable to start server on port " + std::to_string(SERVER_PORT)
};
} else
throw std::runtime_error {
"Unable to open serial port /dev/ttyS0 "
};
}
void Tasks::Run() {
Message *msgRcv;
Message *msgSend;
int counter = 3;
// threadServer=new thread((void (*)(void*)) &Tasks::ServerTask,this);
// threadSendToMon=new thread((void (*)(void*)) &Tasks::SendToMonTask,this);
// threadTimer=new thread((void (*)(void*)) &Tasks::TimerTask,this);
msgSend = ComRobot::Ping();
cout << "Send => " << msgSend->ToString() << endl << flush;
msgRcv = robot.SendCommand(msgSend, MESSAGE_ANSWER_ACK, 3);
cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
delete(msgRcv);
msgSend = ComRobot::StartWithoutWD();
cout << "Send => " << msgSend->ToString() << endl << flush;
msgRcv = robot.SendCommand(msgSend, MESSAGE_ANSWER_ACK, 3);
cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
delete(msgRcv);
msgSend = ComRobot::Move(1000);
cout << "Send => " << msgSend->ToString() << endl << flush;
msgRcv = robot.SendCommand(msgSend, MESSAGE_ANSWER_ACK, 3);
cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
delete(msgRcv);
msgSend = ComRobot::GetBattery();
cout << "Send => " << msgSend->ToString() << endl << flush;
msgRcv = robot.SendCommand(msgSend, MESSAGE_ROBOT_BATTERY_LEVEL, 3);
cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
delete(msgRcv);
}
void Tasks::Stop() {
monitor.Close();
robot.Close();
}
void Tasks::ServerTask(void *arg) {
int err;
/* INIT */
RT_TASK_INFO info;
rt_task_inquire(NULL, &info);
printf("Init %s\n", info.name);
rt_sem_p(&sem_barrier, TM_INFINITE);
err=openServer(DEFAULT_SERVER_PORT);
if (err < 0) {
printf("Failed to start server: %s\n", strerror(-err));
exit(EXIT_FAILURE);
} else {
#ifdef _WITH_TRACE_
printf("%s: server started\n", info.name);
#endif
//Waiting for a client to connect
err=acceptClient();
cout << "Start " << __PRETTY_FUNCTION__ <<endl<<flush;
while (1) {
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);
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) {
#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));
//std::this_thread::sleep_for(std::chrono::seconds )
//sleep(1);
if (nanosleep(&tim, &tim2) < 0) {
printf("Nano sleep system call failed \n");
return;
}
mutexTimer.unlock();
}
}
void f_receiveFromMon(void *arg) {
MessageFromMon msg;
int err;
/* INIT */
RT_TASK_INFO info;
rt_task_inquire(NULL, &info);
printf("Init %s\n", info.name);
rt_sem_p(&sem_barrier, TM_INFINITE);
#ifdef _WITH_TRACE_
printf("%s : waiting for sem_serverOk\n", info.name);
#endif
rt_sem_p(&sem_serverOk, TM_INFINITE);
do {
#ifdef _WITH_TRACE_
printf("%s : waiting for a message from monitor\n", info.name);
#endif
err = receive_message_from_monitor(msg.header, msg.data);
#ifdef _WITH_TRACE_
printf("%s: msg {header:%s,data=%s} received from UI\n", info.name, msg.header, msg.data);
#endif
if (strcmp(msg.header, HEADER_MTS_COM_DMB) == 0) {
if (msg.data[0] == OPEN_COM_DMB) { // Open communication supervisor-robot
#ifdef _WITH_TRACE_
printf("%s: message open Xbee communication\n", info.name);
#endif
rt_sem_v(&sem_openComRobot);
}
} else if (strcmp(msg.header, HEADER_MTS_DMB_ORDER) == 0) {
if (msg.data[0] == DMB_START_WITHOUT_WD) { // Start robot
#ifdef _WITH_TRACE_
printf("%s: message start robot\n", info.name);
#endif
rt_sem_v(&sem_startRobot);
} else if ((msg.data[0] == DMB_GO_BACK)
|| (msg.data[0] == DMB_GO_FORWARD)
|| (msg.data[0] == DMB_GO_LEFT)
|| (msg.data[0] == DMB_GO_RIGHT)
|| (msg.data[0] == DMB_STOP_MOVE)) {
rt_mutex_acquire(&mutex_move, TM_INFINITE);
robotMove = msg.data[0];
rt_mutex_release(&mutex_move);
#ifdef _WITH_TRACE_
printf("%s: message update movement with %c\n", info.name, robotMove);
#endif
}
}
} while (err > 0);
}
void f_openComRobot(void * arg) {
int err;
/* INIT */
RT_TASK_INFO info;
rt_task_inquire(NULL, &info);
printf("Init %s\n", info.name);
rt_sem_p(&sem_barrier, TM_INFINITE);
void Tasks::SendToMonTask(void* arg) {
cout << "Start " << __PRETTY_FUNCTION__ <<endl<<flush;
while (1) {
#ifdef _WITH_TRACE_
printf("%s : Wait sem_openComRobot\n", info.name);
#endif
rt_sem_p(&sem_openComRobot, TM_INFINITE);
#ifdef _WITH_TRACE_
printf("%s : sem_openComRobot arrived => open communication robot\n", info.name);
#endif
err = open_communication_robot();
if (err == 0) {
#ifdef _WITH_TRACE_
printf("%s : the communication is opened\n", info.name);
#endif
MessageToMon msg;
set_msgToMon_header(&msg, (char*)HEADER_STM_ACK);
write_in_queue(&q_messageToMon, msg);
} else {
MessageToMon msg;
set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK);
write_in_queue(&q_messageToMon, msg);
}
}
}
void f_startRobot(void * arg) {
int err;
//void Tasks::f_sendToMon(void * arg) {
// int err;
// MessageToMon msg;
//
// /* INIT */
// RT_TASK_INFO info;
// rt_task_inquire(NULL, &info);
// printf("Init %s\n", info.name);
// rt_sem_p(&sem_barrier, TM_INFINITE);
//
//#ifdef _WITH_TRACE_
// printf("%s : waiting for sem_serverOk\n", info.name);
//#endif
// rt_sem_p(&sem_serverOk, TM_INFINITE);
// while (1) {
//
//#ifdef _WITH_TRACE_
// printf("%s : waiting for a message in queue\n", info.name);
//#endif
// if (rt_queue_read(&q_messageToMon, &msg, sizeof (MessageToRobot), TM_INFINITE) >= 0) {
//#ifdef _WITH_TRACE_
// printf("%s : message {%s,%s} in queue\n", info.name, msg.header, (char*)msg.data);
//#endif
//
// send_message_to_monitor(msg.header, msg.data);
// free_msgToMon_data(&msg);
// rt_queue_free(&q_messageToMon, &msg);
// } else {
// printf("Error msg queue write: %s\n", strerror(-err));
// }
// }
//}
//
//void Tasks::f_receiveFromMon(void *arg) {
// MessageFromMon msg;
// int err;
//
// /* INIT */
// RT_TASK_INFO info;
// rt_task_inquire(NULL, &info);
// printf("Init %s\n", info.name);
// rt_sem_p(&sem_barrier, TM_INFINITE);
//
//#ifdef _WITH_TRACE_
// printf("%s : waiting for sem_serverOk\n", info.name);
//#endif
// rt_sem_p(&sem_serverOk, TM_INFINITE);
// do {
//#ifdef _WITH_TRACE_
// printf("%s : waiting for a message from monitor\n", info.name);
//#endif
// err = receive_message_from_monitor(msg.header, msg.data);
//#ifdef _WITH_TRACE_
// printf("%s: msg {header:%s,data=%s} received from UI\n", info.name, msg.header, msg.data);
//#endif
// if (strcmp(msg.header, HEADER_MTS_COM_DMB) == 0) {
// if (msg.data[0] == OPEN_COM_DMB) { // Open communication supervisor-robot
//#ifdef _WITH_TRACE_
// printf("%s: message open Xbee communication\n", info.name);
//#endif
// rt_sem_v(&sem_openComRobot);
// }
// } else if (strcmp(msg.header, HEADER_MTS_DMB_ORDER) == 0) {
// if (msg.data[0] == DMB_START_WITHOUT_WD) { // Start robot
//#ifdef _WITH_TRACE_
// printf("%s: message start robot\n", info.name);
//#endif
// rt_sem_v(&sem_startRobot);
//
// } else if ((msg.data[0] == DMB_GO_BACK)
// || (msg.data[0] == DMB_GO_FORWARD)
// || (msg.data[0] == DMB_GO_LEFT)
// || (msg.data[0] == DMB_GO_RIGHT)
// || (msg.data[0] == DMB_STOP_MOVE)) {
//
// rt_mutex_acquire(&mutex_move, TM_INFINITE);
// robotMove = msg.data[0];
// rt_mutex_release(&mutex_move);
//#ifdef _WITH_TRACE_
// printf("%s: message update movement with %c\n", info.name, robotMove);
//#endif
//
// }
// }
// } while (err > 0);
//
//}
//
//void Tasks::f_openComRobot(void * arg) {
// int err;
//
// /* INIT */
// RT_TASK_INFO info;
// rt_task_inquire(NULL, &info);
// printf("Init %s\n", info.name);
// rt_sem_p(&sem_barrier, TM_INFINITE);
//
// while (1) {
//#ifdef _WITH_TRACE_
// printf("%s : Wait sem_openComRobot\n", info.name);
//#endif
// rt_sem_p(&sem_openComRobot, TM_INFINITE);
//#ifdef _WITH_TRACE_
// printf("%s : sem_openComRobot arrived => open communication robot\n", info.name);
//#endif
// err = open_communication_robot();
// if (err == 0) {
//#ifdef _WITH_TRACE_
// printf("%s : the communication is opened\n", info.name);
//#endif
// MessageToMon msg;
// set_msgToMon_header(&msg, (char*)HEADER_STM_ACK);
// write_in_queue(&q_messageToMon, msg);
// } else {
// MessageToMon msg;
// set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK);
// write_in_queue(&q_messageToMon, msg);
// }
// }
//}
//
//void Tasks::f_startRobot(void * arg) {
// int err;
//
// /* INIT */
// RT_TASK_INFO info;
// rt_task_inquire(NULL, &info);
// printf("Init %s\n", info.name);
// rt_sem_p(&sem_barrier, TM_INFINITE);
//
// while (1) {
//#ifdef _WITH_TRACE_
// printf("%s : Wait sem_startRobot\n", info.name);
//#endif
// rt_sem_p(&sem_startRobot, TM_INFINITE);
//#ifdef _WITH_TRACE_
// printf("%s : sem_startRobot arrived => Start robot\n", info.name);
//#endif
// err = send_command_to_robot(DMB_START_WITHOUT_WD);
// if (err == 0) {
//#ifdef _WITH_TRACE_
// printf("%s : the robot is started\n", info.name);
//#endif
// rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
// robotStarted = 1;
// rt_mutex_release(&mutex_robotStarted);
// MessageToMon msg;
// set_msgToMon_header(&msg, (char*)HEADER_STM_ACK);
// write_in_queue(&q_messageToMon, msg);
// } else {
// MessageToMon msg;
// set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK);
// write_in_queue(&q_messageToMon, msg);
// }
// }
//}
//
//void Tasks::f_move(void *arg) {
// /* INIT */
// RT_TASK_INFO info;
// rt_task_inquire(NULL, &info);
// printf("Init %s\n", info.name);
// rt_sem_p(&sem_barrier, TM_INFINITE);
//
// /* PERIODIC START */
//#ifdef _WITH_PERIODIC_TRACE_
// printf("%s: start period\n", info.name);
//#endif
// rt_task_set_periodic(NULL, TM_NOW, 100000000);
// while (1) {
//#ifdef _WITH_PERIODIC_TRACE_
// printf("%s: Wait period \n", info.name);
//#endif
// rt_task_wait_period(NULL);
//#ifdef _WITH_PERIODIC_TRACE_
// printf("%s: Periodic activation\n", info.name);
// printf("%s: move equals %c\n", info.name, robotMove);
//#endif
// rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
// if (robotStarted) {
// rt_mutex_acquire(&mutex_move, TM_INFINITE);
// send_command_to_robot(robotMove);
// rt_mutex_release(&mutex_move);
//#ifdef _WITH_TRACE_
// printf("%s: the movement %c was sent\n", info.name, robotMove);
//#endif
// }
// rt_mutex_release(&mutex_robotStarted);
// }
//}
//
//void write_in_queue(RT_QUEUE *queue, MessageToMon msg) {
// void *buff;
// buff = rt_queue_alloc(&q_messageToMon, sizeof (MessageToMon));
// memcpy(buff, &msg, sizeof (MessageToMon));
// rt_queue_send(&q_messageToMon, buff, sizeof (MessageToMon), Q_NORMAL);
//}
/* INIT */
RT_TASK_INFO info;
rt_task_inquire(NULL, &info);
printf("Init %s\n", info.name);
rt_sem_p(&sem_barrier, TM_INFINITE);
while (1) {
#ifdef _WITH_TRACE_
printf("%s : Wait sem_startRobot\n", info.name);
#endif
rt_sem_p(&sem_startRobot, TM_INFINITE);
#ifdef _WITH_TRACE_
printf("%s : sem_startRobot arrived => Start robot\n", info.name);
#endif
err = send_command_to_robot(DMB_START_WITHOUT_WD);
if (err == 0) {
#ifdef _WITH_TRACE_
printf("%s : the robot is started\n", info.name);
#endif
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
robotStarted = 1;
rt_mutex_release(&mutex_robotStarted);
MessageToMon msg;
set_msgToMon_header(&msg, (char*)HEADER_STM_ACK);
write_in_queue(&q_messageToMon, msg);
} else {
MessageToMon msg;
set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK);
write_in_queue(&q_messageToMon, msg);
}
}
}
void f_move(void *arg) {
/* INIT */
RT_TASK_INFO info;
rt_task_inquire(NULL, &info);
printf("Init %s\n", info.name);
rt_sem_p(&sem_barrier, TM_INFINITE);
/* PERIODIC START */
#ifdef _WITH_PERIODIC_TRACE_
printf("%s: start period\n", info.name);
#endif
rt_task_set_periodic(NULL, TM_NOW, 100000000);
while (1) {
#ifdef _WITH_PERIODIC_TRACE_
printf("%s: Wait period \n", info.name);
#endif
rt_task_wait_period(NULL);
#ifdef _WITH_PERIODIC_TRACE_
printf("%s: Periodic activation\n", info.name);
printf("%s: move equals %c\n", info.name, robotMove);
#endif
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
if (robotStarted) {
rt_mutex_acquire(&mutex_move, TM_INFINITE);
send_command_to_robot(robotMove);
rt_mutex_release(&mutex_move);
#ifdef _WITH_TRACE_
printf("%s: the movement %c was sent\n", info.name, robotMove);
#endif
}
rt_mutex_release(&mutex_robotStarted);
}
}
void write_in_queue(RT_QUEUE *queue, MessageToMon msg) {
void *buff;
buff = rt_queue_alloc(&q_messageToMon, sizeof (MessageToMon));
memcpy(buff, &msg, sizeof (MessageToMon));
rt_queue_send(&q_messageToMon, buff, sizeof (MessageToMon), Q_NORMAL);
}
#endif //__WITH_PTHREAD__

163
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/>.
*/
/**
* \file functions.h
* \author PE.Hladik
* \version 1.0
* \date 06/06/2017
* \brief Miscellaneous functions used for destijl project.
*/
#ifndef __TASKS_H__
#define __TASKS_H__
@ -31,73 +23,112 @@
#include <stdlib.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 "robot.h"
#include "image.h"
#include "message.h"
#include "server.h"
#include "camera.h"
#include "img.h"
extern RT_TASK th_server;
extern RT_TASK th_sendToMon;
extern RT_TASK th_receiveFromMon;
extern RT_TASK th_openComRobot;
extern RT_TASK th_startRobot;
extern RT_TASK th_move;
#include "messages.h"
#include "commonitor.h"
#include "comrobot.h"
extern RT_MUTEX mutex_robotStarted;
extern RT_MUTEX mutex_move;
extern RT_SEM sem_barrier;
extern RT_SEM sem_openComRobot;
extern RT_SEM sem_serverOk;
extern RT_SEM sem_startRobot;
extern RT_QUEUE q_messageToMon;
extern int etatCommMoniteur;
extern int robotStarted;
extern char robotMove;
extern int MSG_QUEUE_SIZE;
extern int PRIORITY_TSERVER;
extern int PRIORITY_TOPENCOMROBOT;
extern int PRIORITY_TMOVE;
extern int PRIORITY_TSENDTOMON;
extern int PRIORITY_TRECEIVEFROMMON;
extern int PRIORITY_TSTARTROBOT;
#include <thread>
#include <mutex>
#include <condition_variable>
class Tasks {
public:
/**
* \brief Thread handling server communication.
*/
void f_server(void *arg);
* @brief Initialisation des structures de l'application (tâches, mutex,
* semaphore, etc.)
*/
void Init();
/**
* \brief Thread handling communication to monitor.
*/
void f_sendToMon(void *arg);
/**
* @brief Démarrage des tâches
*/
void Run();
/**
* \brief Thread handling communication from monitor.
*/
void f_receiveFromMon(void *arg);
/**
* @brief Arrêt des tâches
*/
void Stop();
/**
*/
void Join() {
threadServer->join();
threadTimer->join();
threadSendToMon->join();
}
/**
*/
bool AcceptClient() {
return monitor.AcceptClient();
}
/**
* @brief Thread handling server communication.
*/
void ServerTask(void *arg);
/**
* \brief Thread handling opening of robot communication.
*/
void f_openComRobot(void * arg);
/**
* @brief Thread handling server communication.
*/
void TimerTask(void *arg);
/**
* @brief Thread handling communication to monitor.
*/
void SendToMonTask(void *arg);
private:
ComMonitor monitor;
ComRobot robot;
thread *threadServer;
thread *threadSendToMon;
thread *threadTimer;
// thread *threadReceiveFromMon;
// thread *threadOpenComRobot;
// thread *threadStartRobot;
// thread *threadMove;
// thread *threadTimer;
mutex mutexTimer;
// mutex mutexRobotStarted;
// mutex mutexMove;
// mutex semBarrier;
// mutex semOpenComRobot;
// mutex semServerOk;
// mutex semStartRobot;
/**
* \brief Thread handling robot mouvements.
*/
void f_move(void *arg);
/**
* \brief Thread handling robot activation.
*/
void f_startRobot(void *arg);
//
// /**
// * @brief Thread handling communication from monitor.
// */
// void ReceiveFromMonTask(void *arg);
//
// /**
// * @brief Thread handling opening of robot communication.
// */
// void OpenComRobotTask(void * arg);
//
// /**
// * @brief Thread handling robot mouvements.
// */
// void MoveTask(void *arg);
//
// /**
// * @brief Thread handling robot activation.
// */
// void StartRobotTask(void *arg);
};
#endif // __WITH_PTHREAD__
#endif /* __TASKS_H__ */

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
* peut être positif ou negatif.
*
* @param None
* @param NSTART_WITH_WDone
* @retval None
*/
void cmdMoveAction(void) {