vitrat/temps_reel
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

simulateur: ajout temps de réponse et erreur, comrobot : ajout msgAnswer adequat a la reponse

Browse source
This commit is contained in:
pehladik 2020-03-14 23:41:27 +01:00
parent a7eaa150df
commit 7f0a9955db
2 changed files with 140 additions and 78 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -80,12 +80,16 @@ int ComRobot::Open(string usart) {
struct termios options; struct termios options;
#ifdef __SIMULATION__ #ifdef __SIMULATION__
struct sockaddr_in serv_addr; struct sockaddr_in serv_addr;
if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) { if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
printf("\n Socket creation error \n"); printf("\n Socket creation error \n");
return -1; return -1;
} }
struct timeval tv;
tv.tv_sec = 0;
tv.tv_usec = 80000;
setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (const char*) &tv, sizeof tv);
serv_addr.sin_family = AF_INET; serv_addr.sin_family = AF_INET;
serv_addr.sin_port = htons(PORT); serv_addr.sin_port = htons(PORT);
@ -99,10 +103,6 @@ int ComRobot::Open(string usart) {
if (connect(sock, (struct sockaddr *) &serv_addr, sizeof (serv_addr)) < 0) { if (connect(sock, (struct sockaddr *) &serv_addr, sizeof (serv_addr)) < 0) {
return -1; return -1;
} }
/*send(sock , hello , strlen(hello) , 0 );
printf("Hello message sent\n");
valread = read( sock , buffer, 1024);
printf("%s\n",buffer ); */
return 1; return 1;
#else #else
@ -154,11 +154,18 @@ Message *ComRobot::Write(Message* msg) {
#ifdef __SIMULATION__ #ifdef __SIMULATION__
char buffer[1024] = {0}; char buffer[1024] = {0};
cout << "[" <<__PRETTY_FUNCTION__<<"] Send command: "<<s<<endl<<flush; cout << "[" << __PRETTY_FUNCTION__ << "] Send command: " << s << endl << flush;
send(sock, s.c_str(), s.length(), 0); send(sock, s.c_str(), s.length(), 0);
int valread = read(sock, buffer, 1024); int valread = read(sock, buffer, 1024);
msgAnswer = new Message(MESSAGE_ANSWER_ACK); if (valread < 0) {
printf("%s\n", buffer); msgAnswer = new Message(MESSAGE_ANSWER_ROBOT_TIMEOUT);
} else {
string s(&buffer[0], valread);
msgAnswer = StringToMessage(s);
//msgAnswer = new Message(MESSAGE_ANSWER_ACK);
}
cout << "response: " << buffer << ", id: " << msgAnswer->GetID() << endl;
#else #else
AddChecksum(s); AddChecksum(s);

195
software/simulateur/main.cpp
View file

@ -5,6 +5,7 @@
#include <netinet/in.h> #include <netinet/in.h>
#include <string.h> #include <string.h>
#include <iostream> #include <iostream>
#include <time.h>
using namespace std; using namespace std;
const char LABEL_ROBOT_PING = 'p'; const char LABEL_ROBOT_PING = 'p';
@ -25,14 +26,47 @@ const char LABEL_ROBOT_UNKNOWN_COMMAND = 'C';
const char LABEL_ROBOT_SEPARATOR_CHAR = '='; const char LABEL_ROBOT_SEPARATOR_CHAR = '=';
const char LABEL_ROBOT_ENDING_CHAR = 0x0D; const char LABEL_ROBOT_ENDING_CHAR = 0x0D;
int server_fd, new_socket, valread;
#define PORT 6699 #define PORT 6699
int main(int argc, char const *argv[]) { long int ellapse(struct timespec ref, struct timespec cur) {
int server_fd, new_socket, valread; long int e;
e = cur.tv_sec - ref.tv_sec;
e *= 1000000000;
e += cur.tv_nsec - ref.tv_nsec;
return e;
}
void print_time(struct timespec start_time) {
struct timespec t;
clock_gettime(CLOCK_REALTIME, &t);
long int e = ellapse(start_time, t);
fprintf(stdout, "%9ld", e / 1000000);
}
int simulate_error() {
int r = rand() % 1000;
if (r > 950) {
printf(">>> I don't understand what you said (-1)\n");
return -1;
} else if (r > 900) {
printf(">>> I don't want to respond (-2)\n");
return -2;
}
printf(">>> WILCO (0)\n");
return 0;
}
void simulate_transmission_time() {
usleep((rand() % 30) * 1000);
}
void open_server() {
struct sockaddr_in address; struct sockaddr_in address;
int opt = 1; int opt = 1;
int addrlen = sizeof (address); int addrlen = sizeof (address);
char buffer[1024] = {0};
srand(time(NULL));
// Creating socket file descriptor // Creating socket file descriptor
if ((server_fd = socket(AF_INET, SOCK_STREAM, 0)) == 0) { if ((server_fd = socket(AF_INET, SOCK_STREAM, 0)) == 0) {
@ -58,90 +92,111 @@ int main(int argc, char const *argv[]) {
perror("bind failed"); perror("bind failed");
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
cout << ">>> waiting the opening" << endl; cout << ">>> I create a server" << endl;
cout << ">>> ..." << endl;
cout << ">>> I'm waiting a client" << endl;
if (listen(server_fd, 3) < 0) { if (listen(server_fd, 3) < 0) {
perror("listen"); perror("listen");
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
cout << ">>> Hello, I'm Robot" << endl;
if ((new_socket = accept(server_fd, (struct sockaddr *) &address, if ((new_socket = accept(server_fd, (struct sockaddr *) &address,
(socklen_t*) & addrlen)) < 0) { (socklen_t*) & addrlen)) < 0) {
perror("accept"); perror("accept");
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
cout << ">>> The robot is ready to receive something" << endl; }
unsigned long starttime = (unsigned long) time(NULL);
int main(int argc, char const *argv[]) {
char buffer[1024] = {0};
open_server();
cout << ">>> I'm ready to receive something" << endl;
struct timespec start_time;
clock_gettime(CLOCK_REALTIME, &start_time);
while (1) { while (1) {
valread = read(new_socket, buffer, 1024); valread = read(new_socket, buffer, 1024);
if (valread <= 0) if (valread <= 0)
break; break;
printf(">>> I received : %s\n", buffer); print_time(start_time);
cout << (unsigned long) time(NULL) << ": "; printf(" >>> I received a message : %s\n", buffer);
string s = ""; string s = "";
s += LABEL_ROBOT_OK; int error = simulate_error();
switch (buffer[0]) { if (error == 0) {
case LABEL_ROBOT_START_WITHOUT_WD: struct timespec t;
s += LABEL_ROBOT_OK; long int e;
break; switch (buffer[0]) {
case LABEL_ROBOT_START_WITH_WD: case LABEL_ROBOT_START_WITHOUT_WD:
s += LABEL_ROBOT_OK; cout << ">>> I start without watchdog" << endl;
break; s += LABEL_ROBOT_OK;
case LABEL_ROBOT_MOVE: break;
switch (buffer[2]) { case LABEL_ROBOT_START_WITH_WD:
case '0': s += LABEL_ROBOT_OK;
cout << "oo I stop oo" << endl; break;
break; case LABEL_ROBOT_MOVE:
case '-': switch (buffer[2]) {
cout << "\\/ I move backward \\/" << endl; case '0':
break; cout << ">>> XX I stop XX" << endl;
default: break;
cout << "/\\ I move forward /\\" << endl; case '-':
break; cout << ">>> \\/ I move backward \\/" << endl;
} break;
s += LABEL_ROBOT_OK; default:
break; cout << ">>> /\\ I move forward /\\" << endl;
case LABEL_ROBOT_TURN: break;
switch (buffer[2]) {
case '-':
cout << "<< I turn to the left <<" << endl;
break;
default:
cout << ">> I turn to the right >>" << endl;
break;
}
s += LABEL_ROBOT_OK;
break;
case LABEL_ROBOT_GET_BATTERY:
if ((unsigned long) time(NULL) - starttime > 20) {
s += '0';
} else {
if ((unsigned long) time(NULL) - starttime > 10) {
s += '1';
} else {
s += '2';
} }
} s += LABEL_ROBOT_OK;
break;
case LABEL_ROBOT_TURN:
switch (buffer[2]) {
break; case '-':
case '0': cout << ">>> << I turn to the left <<" << endl;
//msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_EMPTY); break;
break; default:
case '1': cout << ">>> >> I turn to the right >>" << endl;
//msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_LOW); break;
break; }
case '2': s += LABEL_ROBOT_OK;
//msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_FULL); break;
break; case LABEL_ROBOT_GET_BATTERY:
default: cout << ">>> I give you my battery level :-o" << endl;
//msg = new Message(MESSAGE_ANSWER_ROBOT_ERROR); clock_gettime(CLOCK_REALTIME, &t);
cerr << "[" << __PRETTY_FUNCTION__ << "] Unknown message received from robot (" << buffer << ")" << endl << flush; e = ellapse(start_time, t);
if (e > 20000000000) {
s += '0';
} else {
if (e > 10000000000) {
s += '1';
} else {
s += '2';
}
}
break;
case LABEL_ROBOT_RELOAD_WD:
cout << ">>> I start with watchdog" << endl;
s += LABEL_ROBOT_OK;
break;
case LABEL_ROBOT_POWEROFF:
cout << ">>> Bye bye" << endl;
s += LABEL_ROBOT_OK;
break;
default:
//msg = new Message(MESSAGE_ANSWER_ROBOT_ERROR);
cerr << "[" << __PRETTY_FUNCTION__ << "] Unknown message received from robot (" << buffer << ")" << endl << flush;
}
simulate_transmission_time();
send(new_socket, s.c_str(), s.length(), 0);
} else if (error == -1){
s += LABEL_ROBOT_UNKNOWN_COMMAND;
simulate_transmission_time();
send(new_socket, s.c_str(), s.length(), 0);
} else if (error == -2){
/* Do nothing */
} }
send(new_socket, s.c_str(), s.length(), 0);
//printf("%s sent\n", s);
} }
cout << "The robot is dead. End of story " << endl; cout << "The robot is dead. End of story. " << endl;
cout << " /\\_/\\" << endl << "( o.o )" << endl << " > ^ <" << endl; cout << " /\\_/\\" << endl << "( o.o )" << endl << " > ^ <" << endl;
return 0; return 0;
} }