diff --git a/software/raspberry/superviseur-robot/lib/comrobot.cpp b/software/raspberry/superviseur-robot/lib/comrobot.cpp index 08f5cff..cbc618a 100644 --- a/software/raspberry/superviseur-robot/lib/comrobot.cpp +++ b/software/raspberry/superviseur-robot/lib/comrobot.cpp @@ -80,12 +80,16 @@ int ComRobot::Open(string usart) { struct termios options; #ifdef __SIMULATION__ - + struct sockaddr_in serv_addr; if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) { printf("\n Socket creation error \n"); return -1; } + struct timeval tv; + tv.tv_sec = 0; + tv.tv_usec = 80000; + setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (const char*) &tv, sizeof tv); serv_addr.sin_family = AF_INET; serv_addr.sin_port = htons(PORT); @@ -99,10 +103,6 @@ int ComRobot::Open(string usart) { if (connect(sock, (struct sockaddr *) &serv_addr, sizeof (serv_addr)) < 0) { return -1; } - /*send(sock , hello , strlen(hello) , 0 ); - printf("Hello message sent\n"); - valread = read( sock , buffer, 1024); - printf("%s\n",buffer ); */ return 1; #else @@ -154,11 +154,18 @@ Message *ComRobot::Write(Message* msg) { #ifdef __SIMULATION__ char buffer[1024] = {0}; - cout << "[" <<__PRETTY_FUNCTION__<<"] Send command: "<GetID() << endl; #else AddChecksum(s); diff --git a/software/simulateur/main.cpp b/software/simulateur/main.cpp index bfc3a79..b486d46 100644 --- a/software/simulateur/main.cpp +++ b/software/simulateur/main.cpp @@ -5,6 +5,7 @@ #include #include #include +#include using namespace std; 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_ENDING_CHAR = 0x0D; +int server_fd, new_socket, valread; #define PORT 6699 -int main(int argc, char const *argv[]) { - int server_fd, new_socket, valread; +long int ellapse(struct timespec ref, struct timespec cur) { + 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; int opt = 1; int addrlen = sizeof (address); - char buffer[1024] = {0}; + + srand(time(NULL)); // Creating socket file descriptor if ((server_fd = socket(AF_INET, SOCK_STREAM, 0)) == 0) { @@ -58,90 +92,111 @@ int main(int argc, char const *argv[]) { perror("bind failed"); 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) { perror("listen"); exit(EXIT_FAILURE); } + + cout << ">>> Hello, I'm Robot" << endl; if ((new_socket = accept(server_fd, (struct sockaddr *) &address, (socklen_t*) & addrlen)) < 0) { perror("accept"); 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) { valread = read(new_socket, buffer, 1024); if (valread <= 0) break; - printf(">>> I received : %s\n", buffer); - cout << (unsigned long) time(NULL) << ": "; + print_time(start_time); + printf(" >>> I received a message : %s\n", buffer); string s = ""; - s += LABEL_ROBOT_OK; - switch (buffer[0]) { - case LABEL_ROBOT_START_WITHOUT_WD: - s += LABEL_ROBOT_OK; - break; - case LABEL_ROBOT_START_WITH_WD: - s += LABEL_ROBOT_OK; - break; - case LABEL_ROBOT_MOVE: - switch (buffer[2]) { - case '0': - cout << "oo I stop oo" << endl; - break; - case '-': - cout << "\\/ I move backward \\/" << endl; - break; - default: - cout << "/\\ I move forward /\\" << endl; - break; - } - s += LABEL_ROBOT_OK; - break; - case LABEL_ROBOT_TURN: - 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'; + int error = simulate_error(); + if (error == 0) { + struct timespec t; + long int e; + switch (buffer[0]) { + case LABEL_ROBOT_START_WITHOUT_WD: + cout << ">>> I start without watchdog" << endl; + s += LABEL_ROBOT_OK; + break; + case LABEL_ROBOT_START_WITH_WD: + s += LABEL_ROBOT_OK; + break; + case LABEL_ROBOT_MOVE: + switch (buffer[2]) { + case '0': + cout << ">>> XX I stop XX" << endl; + break; + case '-': + cout << ">>> \\/ I move backward \\/" << endl; + break; + default: + cout << ">>> /\\ I move forward /\\" << endl; + break; } - } - - - - 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 (" << buffer << ")" << endl << flush; + s += LABEL_ROBOT_OK; + break; + case LABEL_ROBOT_TURN: + 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: + cout << ">>> I give you my battery level :-o" << endl; + clock_gettime(CLOCK_REALTIME, &t); + 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; return 0; } \ No newline at end of file