Browse Source

Classe comRobot partiellement debuggée

Sébastien DI MERCURIO 5 years ago
parent
commit
7f0be2d5fd
23 changed files with 1170 additions and 1373 deletions
  1. BIN
      software/raspberry/superviseur-robot/dist/Debug__Pthread_/GNU-Linux/superviseur-robot
  2. 1
    1
      software/raspberry/superviseur-robot/lib/camera.cpp
  3. 98
    105
      software/raspberry/superviseur-robot/lib/commonitor.cpp
  4. 2
    0
      software/raspberry/superviseur-robot/lib/commonitor.h
  5. 247
    225
      software/raspberry/superviseur-robot/lib/comrobot.cpp
  6. 115
    60
      software/raspberry/superviseur-robot/lib/comrobot.h
  7. 12
    8
      software/raspberry/superviseur-robot/lib/img.cpp
  8. 1
    1
      software/raspberry/superviseur-robot/lib/img.h
  9. 26
    205
      software/raspberry/superviseur-robot/lib/messages.cpp
  10. 29
    164
      software/raspberry/superviseur-robot/lib/messages.h
  11. 21
    178
      software/raspberry/superviseur-robot/main.cpp
  12. 12
    0
      software/raspberry/superviseur-robot/nbproject/Makefile-Debug.mk
  13. 14
    26
      software/raspberry/superviseur-robot/nbproject/Makefile-Debug__Pthread_.mk
  14. 12
    0
      software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk
  15. 12
    0
      software/raspberry/superviseur-robot/nbproject/Makefile-Release.mk
  16. 47
    23
      software/raspberry/superviseur-robot/nbproject/configurations.xml
  17. 2
    0
      software/raspberry/superviseur-robot/nbproject/private/configurations.xml
  18. 3
    13
      software/raspberry/superviseur-robot/nbproject/private/private.xml
  19. 13
    9
      software/raspberry/superviseur-robot/tasks.cpp
  20. 104
    75
      software/raspberry/superviseur-robot/tasks.h
  21. 297
    209
      software/raspberry/superviseur-robot/tasks_pthread.cpp
  22. 101
    70
      software/raspberry/superviseur-robot/tasks_pthread.h
  23. 1
    1
      software/robot/src/cmdManager.c

BIN
software/raspberry/superviseur-robot/dist/Debug__Pthread_/GNU-Linux/superviseur-robot View File


+ 1
- 1
software/raspberry/superviseur-robot/lib/camera.cpp View File

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

+ 98
- 105
software/raspberry/superviseur-robot/lib/commonitor.cpp View File

@@ -33,25 +33,25 @@
33 33
  */
34 34
 const string LABEL_MONITOR_ANSWER_ACK = "AACK";
35 35
 const string LABEL_MONITOR_ANSWER_NACK = "ANAK";
36
-const string LABEL_MONITOR_ANSWER_LOST_DMB= "ATIM";
37
-const string LABEL_MONITOR_ANSWER_TIMEOUT= "ATIM";
38
-const string LABEL_MONITOR_ANSWER_CMD_REJECTED= "ACRJ";
36
+const string LABEL_MONITOR_ANSWER_COM_ERROR = "ACER";
37
+const string LABEL_MONITOR_ANSWER_TIMEOUT = "ATIM";
38
+const string LABEL_MONITOR_ANSWER_CMD_REJECTED = "ACRJ";
39 39
 const string LABEL_MONITOR_MESSAGE = "MSSG";
40
-const string LABEL_MONITOR_CAMERA_OPEN= "COPN";
41
-const string LABEL_MONITOR_CAMERA_CLOSE= "CCLS";
40
+const string LABEL_MONITOR_CAMERA_OPEN = "COPN";
41
+const string LABEL_MONITOR_CAMERA_CLOSE = "CCLS";
42 42
 const string LABEL_MONITOR_CAMERA_IMAGE = "CIMG";
43 43
 const string LABEL_MONITOR_CAMERA_ARENA_ASK = "CASA";
44 44
 const string LABEL_MONITOR_CAMERA_ARENA_INFIRME = "CAIN";
45 45
 const string LABEL_MONITOR_CAMERA_ARENA_CONFIRM = "CACO";
46
-const string LABEL_MONITOR_CAMERA_POSITION_COMPUTE= "CPCO";
47
-const string LABEL_MONITOR_CAMERA_POSITION_STOP= "CPST";
46
+const string LABEL_MONITOR_CAMERA_POSITION_COMPUTE = "CPCO";
47
+const string LABEL_MONITOR_CAMERA_POSITION_STOP = "CPST";
48 48
 const string LABEL_MONITOR_CAMERA_POSITION = "CPOS";
49 49
 const string LABEL_MONITOR_ROBOT_COM_OPEN = "ROPN";
50 50
 const string LABEL_MONITOR_ROBOT_COM_CLOSE = "RCLS";
51 51
 const string LABEL_MONITOR_ROBOT_PING = "RPIN";
52 52
 const string LABEL_MONITOR_ROBOT_RESET = "RRST";
53
-const string LABEL_MONITOR_ROBOT_START_WITHOUT_WD= "RSOW";
54
-const string LABEL_MONITOR_ROBOT_START_WITH_WD= "RSWW";
53
+const string LABEL_MONITOR_ROBOT_START_WITHOUT_WD = "RSOW";
54
+const string LABEL_MONITOR_ROBOT_START_WITH_WD = "RSWW";
55 55
 const string LABEL_MONITOR_ROBOT_RELOAD_WD = "RLDW";
56 56
 const string LABEL_MONITOR_ROBOT_MOVE = "RMOV";
57 57
 const string LABEL_MONITOR_ROBOT_TURN = "RTRN";
@@ -80,7 +80,7 @@ int ComMonitor::Open(int port) {
80 80
 
81 81
     socketFD = socket(AF_INET, SOCK_STREAM, 0);
82 82
     if (socketFD < 0) {
83
-        throw std::runtime_error{"ComMonitor::Open : Can not create socket"};
83
+        throw std::runtime_error{"Can not create socket"};
84 84
     }
85 85
 
86 86
     server.sin_addr.s_addr = INADDR_ANY;
@@ -88,7 +88,8 @@ int ComMonitor::Open(int port) {
88 88
     server.sin_port = htons(port);
89 89
 
90 90
     if (bind(socketFD, (struct sockaddr *) &server, sizeof (server)) < 0) {
91
-        throw std::runtime_error{"ComMonitor::Open : Can not bind socket on port " + std::to_string(port)};
91
+        cerr<<"["<<__PRETTY_FUNCTION__<<"] Can not bind socket ("<<to_string(port)<<")"<<endl<<flush;
92
+        throw std::runtime_error{"Can not bind socket"};
92 93
     }
93 94
 
94 95
     listen(socketFD, 1);
@@ -117,9 +118,7 @@ int ComMonitor::AcceptClient() {
117 118
     clientID = accept(socketFD, (struct sockaddr *) &client, (socklen_t*) & c);
118 119
 
119 120
     if (clientID < 0)
120
-        throw std::runtime_error {
121
-        "ComMonitor::AcceptClient : Accept failed"
122
-    };
121
+        throw std::runtime_error {"Accept failed"};
123 122
 
124 123
     return clientID;
125 124
 }
@@ -144,7 +143,7 @@ void ComMonitor::Write(Message &msg) {
144 143
     write(clientID, str.c_str(), str.length());
145 144
 
146 145
     delete(&msg);
147
-    
146
+
148 147
     // Call user method after write
149 148
     Write_Post();
150 149
 }
@@ -160,9 +159,9 @@ Message *ComMonitor::Read() {
160 159
     char length = 0;
161 160
     string s;
162 161
     char data;
163
-    bool endReception=false;
162
+    bool endReception = false;
164 163
     Message *msg;
165
-    
164
+
166 165
     // Call user method before read
167 166
     Read_Pre();
168 167
 
@@ -170,20 +169,20 @@ Message *ComMonitor::Read() {
170 169
         while (!endReception) {
171 170
             if ((length = recv(clientID, (void*) &data, 1, MSG_WAITALL)) > 0) {
172 171
                 if (data != '\n') {
173
-                    s+=data;
172
+                    s += data;
174 173
                 } else endReception = true;
175 174
             }
176 175
         }
177 176
 
178
-        if (length<=0) msg = new Message(MESSAGE_MONITOR_LOST);
177
+        if (length <= 0) msg = new Message(MESSAGE_MONITOR_LOST);
179 178
         else {
180
-            msg=StringToMessage(s);
179
+            msg = StringToMessage(s);
181 180
         }
182 181
     }
183 182
 
184 183
     // Call user method after read
185 184
     Read_Post();
186
-    
185
+
187 186
     return msg;
188 187
 }
189 188
 
@@ -197,62 +196,56 @@ string ComMonitor::MessageToString(Message &msg) {
197 196
     string str;
198 197
     Message *localMsg = &msg;
199 198
     Position pos;
200
-    
199
+
201 200
     id = msg.GetID();
202 201
 
203 202
     switch (id) {
204
-        case MESSAGE_ANSWER:
205
-            switch (((MessageAnswer*)localMsg)->GetAnswer()) {
206
-                case ANSWER_ACK:
207
-                    str.append(LABEL_MONITOR_ANSWER_ACK);
208
-                    break;
209
-                case ANSWER_NACK:
210
-                    str.append(LABEL_MONITOR_ANSWER_NACK);
211
-                    break;
212
-                case ANSWER_LOST_ROBOT:
213
-                    str.append(LABEL_MONITOR_ANSWER_LOST_DMB);
214
-                    break;
215
-                case ANSWER_ROBOT_TIMEOUT:
216
-                    str.append(LABEL_MONITOR_ANSWER_TIMEOUT);
217
-                    break;
218
-                case ANSWER_ROBOT_UNKNOWN_COMMAND:
219
-                    str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED);
220
-                    break;
221
-                case ANSWER_ROBOT_ERROR:
222
-                    str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED);
223
-                    break;
224
-                default:
225
-                    str.append(LABEL_MONITOR_ANSWER_NACK);
226
-            };
227
-            
203
+        case MESSAGE_ANSWER_ACK :
204
+            str.append(LABEL_MONITOR_ANSWER_ACK);
205
+            break;
206
+        case MESSAGE_ANSWER_NACK:
207
+            str.append(LABEL_MONITOR_ANSWER_NACK);
208
+            break;
209
+        case MESSAGE_ANSWER_ROBOT_TIMEOUT:
210
+            str.append(LABEL_MONITOR_ANSWER_TIMEOUT);
211
+            break;
212
+        case MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND:
213
+            str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED);
228 214
             break;
229
-        case MESSAGE_POSITION:
230
-            pos = ((MessagePosition*)&msg)->GetPosition();
231
-            
215
+        case MESSAGE_ANSWER_ROBOT_ERROR:
216
+            str.append(LABEL_MONITOR_ANSWER_CMD_REJECTED);
217
+            break;
218
+        case MESSAGE_ANSWER_COM_ERROR:
219
+            str.append(LABEL_MONITOR_ANSWER_COM_ERROR);
220
+            break;
221
+        case MESSAGE_CAM_POSITION:
222
+            pos = ((MessagePosition*) & msg)->GetPosition();
223
+
232 224
             str.append(LABEL_MONITOR_CAMERA_POSITION + LABEL_SEPARATOR_CHAR + to_string(pos.robotId) + ";" +
233 225
                     to_string(pos.angle) + ";" + to_string(pos.center.x) + ";" + to_string(pos.center.y) + ";" +
234 226
                     to_string(pos.direction.x) + ";" + to_string(pos.direction.y));
235 227
             break;
236
-        case MESSAGE_IMAGE:
237
-            str.append(LABEL_MONITOR_CAMERA_IMAGE + LABEL_SEPARATOR_CHAR + ((MessageImg*) &msg)->GetImage()->ToBase64());
228
+        case MESSAGE_CAM_IMAGE:
229
+            str.append(LABEL_MONITOR_CAMERA_IMAGE + LABEL_SEPARATOR_CHAR + ((MessageImg*) & msg)->GetImage()->ToBase64());
238 230
             break;
239 231
         case MESSAGE_ROBOT_BATTERY_LEVEL:
240
-            str.append(LABEL_MONITOR_ROBOT_BATTERY_LEVEL + LABEL_SEPARATOR_CHAR + to_string(((MessageBattery*) &msg)->GetLevel()));
232
+            str.append(LABEL_MONITOR_ROBOT_BATTERY_LEVEL + LABEL_SEPARATOR_CHAR + to_string(((MessageBattery*) & msg)->GetLevel()));
233
+            break;
234
+        case MESSAGE_ROBOT_STATE_BUSY:
235
+            str.append(LABEL_MONITOR_ROBOT_CURRENT_STATE + LABEL_SEPARATOR_CHAR + "1");
241 236
             break;
242
-        case MESSAGE_ROBOT_CURRENT_STATE:
243
-            str.append(LABEL_MONITOR_ROBOT_CURRENT_STATE + LABEL_SEPARATOR_CHAR + to_string(((MessageState*) &msg)->GetState()));
237
+        case MESSAGE_ROBOT_STATE_NOT_BUSY:
238
+            str.append(LABEL_MONITOR_ROBOT_CURRENT_STATE + LABEL_SEPARATOR_CHAR + "0");
244 239
             break;
245 240
         case MESSAGE_LOG:
246
-            str.append(LABEL_MONITOR_MESSAGE + LABEL_SEPARATOR_CHAR + ((MessageString*) &msg)->GetString());
241
+            str.append(LABEL_MONITOR_MESSAGE + LABEL_SEPARATOR_CHAR + ((MessageString*) & msg)->GetString());
247 242
             break;
248 243
         case MESSAGE_EMPTY:
249 244
             str.append(""); //empty string
250 245
             break;
251 246
         default:
252
-            throw std::runtime_error
253
-        {
254
-            "ComMonitor::MessageToString (from ComMonitor::Write): Invalid message to send (" + msg.ToString()
255
-        };
247
+            cerr<<"["<<__PRETTY_FUNCTION__<<"] (from ComMonitor::Write): Invalid message to send ("<<msg.ToString()<<")"<<endl<<flush;
248
+            throw std::runtime_error {"Invalid message to send"};
256 249
     }
257 250
 
258 251
     str.append("\n");
@@ -268,69 +261,69 @@ string ComMonitor::MessageToString(Message &msg) {
268 261
 Message *ComMonitor::StringToMessage(string &s) {
269 262
     Message *msg;
270 263
     size_t pos;
271
-    string org =s;
264
+    string org = s;
272 265
     string tokenCmd;
273 266
     string tokenData;
274
-    
267
+
275 268
     /* Separate command from data if string contains a ':' */
276
-    if ((pos=org.find(LABEL_SEPARATOR_CHAR)) != string::npos) {
277
-        tokenCmd = org.substr(0,pos);
278
-        org.erase(0,pos+1);
279
-        tokenData=org;
280
-    } else tokenCmd=org;
281
-    
269
+    if ((pos = org.find(LABEL_SEPARATOR_CHAR)) != string::npos) {
270
+        tokenCmd = org.substr(0, pos);
271
+        org.erase(0, pos + 1);
272
+        tokenData = org;
273
+    } else tokenCmd = org;
274
+
282 275
     /* Convert command to message */
283
-    if (tokenCmd.find(LABEL_MONITOR_ROBOT_MOVE)!= string::npos) {
284
-        msg = new MessageInt(MESSAGE_ROBOT_MOVE,stoi(tokenData));
285
-    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_TURN)!= string::npos) {
286
-        msg = new MessageInt(MESSAGE_ROBOT_TURN,stoi(tokenData));
287
-    } else  if (tokenCmd.find(LABEL_MONITOR_ROBOT_START_WITHOUT_WD)!= string::npos) {
276
+    if (tokenCmd.find(LABEL_MONITOR_ROBOT_MOVE) != string::npos) {
277
+        msg = new MessageInt(MESSAGE_ROBOT_MOVE, stoi(tokenData));
278
+    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_TURN) != string::npos) {
279
+        msg = new MessageInt(MESSAGE_ROBOT_TURN, stoi(tokenData));
280
+    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_START_WITHOUT_WD) != string::npos) {
288 281
         msg = new Message(MESSAGE_ROBOT_START_WITHOUT_WD);
289
-    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_START_WITH_WD)!= string::npos) {
282
+    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_START_WITH_WD) != string::npos) {
290 283
         msg = new Message(MESSAGE_ROBOT_START_WITH_WD);
291
-    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_RELOAD_WD)!= string::npos) {
284
+    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_RELOAD_WD) != string::npos) {
292 285
         msg = new Message(MESSAGE_ROBOT_RELOAD_WD);
293
-    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_PING)!= string::npos) {
286
+    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_PING) != string::npos) {
294 287
         msg = new Message(MESSAGE_ROBOT_PING);
295
-    } else  if (tokenCmd.find(LABEL_MONITOR_ROBOT_RESET)!= string::npos) {
288
+    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_RESET) != string::npos) {
296 289
         msg = new Message(MESSAGE_ROBOT_RESET);
297
-    } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_ASK)!= string::npos) {
298
-        msg = new Message(MESSAGE_ASK_ARENA);
299
-    } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_CONFIRM)!= string::npos) {
300
-        msg = new Message(MESSAGE_ARENA_CONFIRM);
301
-    } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_INFIRME)!= string::npos) {
302
-        msg = new Message(MESSAGE_ARENA_INFIRM);
303
-    } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_CLOSE)!= string::npos) {
290
+    } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_ASK) != string::npos) {
291
+        msg = new Message(MESSAGE_CAM_ASK_ARENA);
292
+    } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_CONFIRM) != string::npos) {
293
+        msg = new Message(MESSAGE_CAM_ARENA_CONFIRM);
294
+    } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_ARENA_INFIRME) != string::npos) {
295
+        msg = new Message(MESSAGE_CAM_ARENA_INFIRM);
296
+    } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_CLOSE) != string::npos) {
304 297
         msg = new Message(MESSAGE_CAM_CLOSE);
305
-    } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_OPEN)!= string::npos) {
298
+    } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_OPEN) != string::npos) {
306 299
         msg = new Message(MESSAGE_CAM_OPEN);
307
-    } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_POSITION_COMPUTE)!= string::npos) {
308
-        msg = new Message(MESSAGE_COMPUTE_POSITION);
309
-    } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_POSITION_STOP)!= string::npos) {
310
-        msg = new Message(MESSAGE_STOP_COMPUTE_POSITION);
311
-    } else if (tokenCmd.find(LABEL_MONITOR_MESSAGE)!= string::npos) {
312
-        msg = new MessageString(MESSAGE_LOG,tokenData);
313
-    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_COM_CLOSE)!= string::npos) {
314
-        msg = new Message(MESSAGE_CLOSE_COM);
315
-    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_COM_OPEN)!= string::npos) {
316
-        msg = new Message(MESSAGE_OPEN_COM);
317
-    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GET_BATTERY)!= string::npos) {
318
-        msg = new Message(MESSAGE_ROBOT_GET_BATTERY);
319
-    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GET_STATE)!= string::npos) {
320
-        msg = new Message(MESSAGE_ROBOT_GET_STATE);
321
-    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_FORWARD)!= string::npos) {
300
+    } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_POSITION_COMPUTE) != string::npos) {
301
+        msg = new Message(MESSAGE_CAM_POSITION_COMPUTE_START);
302
+    } else if (tokenCmd.find(LABEL_MONITOR_CAMERA_POSITION_STOP) != string::npos) {
303
+        msg = new Message(MESSAGE_CAM_POSITION_COMPUTE_STOP);
304
+    } else if (tokenCmd.find(LABEL_MONITOR_MESSAGE) != string::npos) {
305
+        msg = new MessageString(MESSAGE_LOG, tokenData);
306
+    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_COM_CLOSE) != string::npos) {
307
+        msg = new Message(MESSAGE_ROBOT_COM_CLOSE);
308
+    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_COM_OPEN) != string::npos) {
309
+        msg = new Message(MESSAGE_ROBOT_COM_OPEN);
310
+    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GET_BATTERY) != string::npos) {
311
+        msg = new Message(MESSAGE_ROBOT_BATTERY_GET);
312
+    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GET_STATE) != string::npos) {
313
+        msg = new Message(MESSAGE_ROBOT_STATE_GET);
314
+    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_FORWARD) != string::npos) {
322 315
         msg = new Message(MESSAGE_ROBOT_GO_FORWARD);
323
-    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_BACKWARD)!= string::npos) {
324
-        msg = new Message(MESSAGE_ROBOT_GO_BACK);
325
-    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_LEFT)!= string::npos) {
316
+    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_BACKWARD) != string::npos) {
317
+        msg = new Message(MESSAGE_ROBOT_GO_BACKWARD);
318
+    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_LEFT) != string::npos) {
326 319
         msg = new Message(MESSAGE_ROBOT_GO_LEFT);
327
-    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_RIGHT)!= string::npos) {
320
+    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_GO_RIGHT) != string::npos) {
328 321
         msg = new Message(MESSAGE_ROBOT_GO_RIGHT);
329
-    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_POWEROFF)!= string::npos) {
322
+    } else if (tokenCmd.find(LABEL_MONITOR_ROBOT_POWEROFF) != string::npos) {
330 323
         msg = new Message(MESSAGE_ROBOT_POWEROFF);
331 324
     } else {
332 325
         msg = new Message(MESSAGE_EMPTY);
333 326
     }
334
-    
327
+
335 328
     return msg;
336 329
 }

+ 2
- 0
software/raspberry/superviseur-robot/lib/commonitor.h View File

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

+ 247
- 225
software/raspberry/superviseur-robot/lib/comrobot.cpp View File

@@ -35,16 +35,23 @@
35 35
 /*
36 36
  * Constants to be used for communicating with robot. Contains command tag
37 37
  */
38
-typedef enum {
39
-    LABEL_ANGLE_POSITION = 'p',
40
-    LABEL_ANGULAR_SPEED = 's',
41
-    LABEL_BATTERY_LEVEL = 'b',
42
-    LABEL_BETA_ANGLE = 'v',
43
-    LABEL_USER_PRESENCE = 'u',
44
-
45
-    LABEL_TORQUE = 'c',
46
-    LABEL_EMERGENCY_STOP = 'a'
47
-} LabelRobot;
38
+const char LABEL_ROBOT_PING = 'p';
39
+const char LABEL_ROBOT_RESET = 'r';
40
+const char LABEL_ROBOT_START_WITH_WD = 'W';
41
+const char LABEL_ROBOT_START_WITHOUT_WD = 'u';
42
+const char LABEL_ROBOT_RELOAD_WD = 'w';
43
+const char LABEL_ROBOT_MOVE = 'M';
44
+const char LABEL_ROBOT_TURN = 'T';
45
+const char LABEL_ROBOT_GET_BATTERY = 'v';
46
+const char LABEL_ROBOT_GET_STATE = 'b';
47
+const char LABEL_ROBOT_POWEROFF = 'z';
48
+
49
+const char LABEL_ROBOT_OK = 'O';
50
+const char LABEL_ROBOT_ERROR = 'E';
51
+const char LABEL_ROBOT_UNKNOWN_COMMAND = 'C';
52
+
53
+const char LABEL_ROBOT_SEPARATOR_CHAR = '=';
54
+const char LABEL_ROBOT_ENDING_CHAR = 0x0D; // carriage return (\\r)
48 55
 
49 56
 /**
50 57
  * Open serial link with robot
@@ -52,22 +59,35 @@ typedef enum {
52 59
  * @throw std::runtime_error if it fails
53 60
  */
54 61
 int ComRobot::Open() {
55
-    fd = open(USART_FILENAME, O_RDWR | O_NOCTTY /*| O_NDELAY*/); //Open in blocking read/write mode
62
+    return this->Open(USART_FILENAME);
63
+}
64
+
65
+/**
66
+ * Open serial link with robot
67
+ * @param usart Filename of usart to open
68
+ * @return File descriptor
69
+ * @throw std::runtime_error if it fails
70
+ */
71
+int ComRobot::Open(string usart) {
72
+    struct termios options;
73
+    
74
+    fd = open(usart.c_str(), O_RDWR | O_NOCTTY /*| O_NDELAY*/); //Open in blocking read/write mode
56 75
     if (fd == -1) {
57
-        //ERROR - CAN'T OPEN SERIAL PORT
58
-        throw std::runtime_error{"Error - Unable to open UART " + string(USART_FILENAME) + ".  Ensure it is not in use by another application"};
76
+        cerr<<"["<<__PRETTY_FUNCTION__<<"] Unable to open UART ("<<usart<<"). Ensure it is not in use by another application"<<endl<<flush;
77
+        throw std::runtime_error{"Unable to open UART"};
59 78
         exit(EXIT_FAILURE);
60 79
     }
61
-
62
-    //Configuration of the serial port 115 520 Bauds
63
-    struct termios options;
64
-    tcgetattr(fd, &options);
65
-    options.c_cflag = B115200 | CS8 | CLOCAL | CREAD; //<Set baud rate
66
-    options.c_iflag = IGNPAR; // ignores bytes with bad parity
67
-    options.c_oflag = 0;
68
-    options.c_lflag = 0;
69
-    tcflush(fd, TCIFLUSH);
70
-    tcsetattr(fd, TCSANOW, &options);
80
+    else
81
+    {
82
+        fcntl(fd, F_SETFL, 0);
83
+        tcgetattr(fd, &options);
84
+        options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
85
+        cfsetospeed (&options, B9600);
86
+        cfsetispeed (&options, B9600);
87
+        options.c_cc[VMIN]=0;
88
+        options.c_cc[VTIME]=1; /* Timeout of 100 ms per character*/
89
+        tcsetattr(fd, TCSANOW, &options);
90
+    }
71 91
 
72 92
     return fd;
73 93
 }
@@ -79,6 +99,61 @@ int ComRobot::Open() {
79 99
 int ComRobot::Close() {
80 100
     return close(fd);
81 101
 }
102
+/**
103
+ * Send a message to robot
104
+ * @param msg Message to send to robot
105
+ * @return 1 if success, 0 otherwise
106
+ * @attention Message is destroyed (delete) after being sent. You do not need to delete it yourself
107
+ * @attention Write is blocking until message is written into buffer (linux side)
108
+ * @warning Write is not thread save : check that multiple tasks can't access this method simultaneously  
109
+ */
110
+Message *ComRobot::Write(Message* msg) {
111
+    Message *msgAnswer;
112
+    string s;
113
+
114
+    if (this->fd != -1) {
115
+
116
+        Write_Pre();
117
+
118
+        s=MessageToString(msg);
119
+        AddChecksum(s);
120
+
121
+        //cout << "[" <<__PRETTY_FUNCTION__<<"] Send command: "<<s<<endl<<flush;
122
+        int count = write(this->fd, s.c_str(), s.length()); //Filestream, bytes to write, number of bytes to write
123
+
124
+        if (count < 0) {
125
+            cerr << "[" << __PRETTY_FUNCTION__ << "] UART TX error (" << to_string(count) << ")" << endl << flush;
126
+            msgAnswer = new Message(MESSAGE_ANSWER_COM_ERROR);
127
+        } else { /* write successfull, read answer from robot */
128
+
129
+            try {
130
+                s = Read();
131
+                cout << "Answer = "<<s<<endl<<flush;
132
+                
133
+                if (VerifyChecksum(s)) {
134
+                    msgAnswer = StringToMessage(s);
135
+                } else msgAnswer = new Message(MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND);
136
+
137
+            } catch (std::runtime_error &e) {
138
+                s = string(e.what());
139
+
140
+                if (s.find("imeout")) { // timeout detecté
141
+                    msgAnswer = new Message(MESSAGE_ANSWER_ROBOT_TIMEOUT);
142
+                } else {
143
+                    msgAnswer = new Message(MESSAGE_ANSWER_COM_ERROR);
144
+                }
145
+            }
146
+        }
147
+    } else {
148
+        cerr << __PRETTY_FUNCTION__ << ": Com port not open" << endl << flush;
149
+        throw std::runtime_error{"Com port not open"};
150
+    }
151
+
152
+    // deallocation of msg
153
+    delete(msg);
154
+
155
+    return msgAnswer;
156
+}
82 157
 
83 158
 /**
84 159
  * Get a message from robot
@@ -87,63 +162,52 @@ int ComRobot::Close() {
87 162
  * @attention Read method is blocking until a message is received
88 163
  * @warning Read is not thread safe : Do not call it in multiple tasks simultaneously
89 164
  */
90
-Message* ComRobot::Read() {
165
+string ComRobot::Read() {
166
+    string s;
91 167
     int rxLength;
92
-    unsigned char rxBuffer[6];
93 168
     unsigned char receivedChar;
94
-    bool messageComplete = false;
95
-    Message *msg;
96
-    unsigned int i;
97
-
98
-    /* Call pre method for read */
99
-    Read_Pre();
100
-
101
-    /* a message is composed of 7 bytes.
102
-                the byte 0 should always be '<'
103
-                the byte 1 should be an ascii char that is the label. It define what the data represent
104
-                the bytes 2 to 5 are the float value
105
-                the byte 6 should always be a '\n'
106
-     */
107
-    while (messageComplete == false) {
108
-        rxLength = read(this->fd, (void*) &receivedChar, 1); //Filestream, buffer to store in, number of bytes to read (max)
109
-        //printf ("W=%02X ", receivedChar);
110
-
111
-        if (rxLength <= -1) {
112
-            this->lostCom = true;
113
-            printf("Warning: communication lost in ComStm32::Read\n");
114
-            msg = new Message();
115
-
116
-            return msg;
117
-        } else if (rxLength == 0) {
118
-            // nothing to do
119
-        } else if (receivedChar == '<') { // start of frame received
120
-            i = 0;
121 169
 
122
-            do {
123
-                rxLength = read(this->fd, (void*) &rxBuffer[i], 6 - i); //Filestream, buffer to store in, number of bytes to read (max)
124
-
125
-                if (rxLength >= 0)
126
-                    i = i + rxLength;
127
-                else {
128
-                    printf("Error while reading (%i)", rxLength);
129
-
130
-                    return NULL;
131
-                }
132
-            } while (i < 6);
133
-
134
-            if (rxBuffer[5] == '\n') {
135
-                messageComplete = true;
170
+    do {
171
+        rxLength = read(this->fd, (void*) &receivedChar, 1); //Filestream, buffer to store in, number of bytes to read (max)
172
+        if (rxLength ==0) { // timeout
173
+            // try again
174
+            rxLength = read(this->fd, (void*) &receivedChar, 1); //Filestream, buffer to store in, number of bytes to read (max)
175
+            if (rxLength ==0) { // re-timeout: it sucks !
176
+                throw std::runtime_error {"ComRobot::Read: Timeout when reading from com port"};
136 177
             }
178
+        } else if (rxLength <0) { // big pb !
179
+            throw std::runtime_error {"ComRobot::Read: Unknown problem when reading from com port"};
180
+        } else { // everything ok
181
+            if ((receivedChar != '\r') && (receivedChar != '\n')) s += receivedChar;
137 182
         }
138
-    }
183
+    } while ((receivedChar != '\r') && (receivedChar != '\n'));
139 184
 
140
-    /* Treatment of received message */
141
-    msg = CharToMessage(rxBuffer);
142
-
143
-    /* Call Post method for read */
144
-    Read_Post();
185
+    return s;
186
+}
145 187
 
146
-    return msg;
188
+Message *ComRobot::SendCommand(Message* msg, MessageID answerID, int maxRetries) {
189
+    int counter = maxRetries;
190
+    Message *msgSend;
191
+    Message *msgRcv;
192
+    Message *msgTmp;
193
+    
194
+    do {
195
+        msgSend = msg->Copy();
196
+        cout << "S => " << msgSend->ToString() << endl << flush;
197
+        msgTmp = Write(msgSend);
198
+        cout << "R <= " << msgTmp->ToString() << endl << flush;
199
+
200
+        if (msgTmp->CompareID(answerID)) counter = 0;
201
+        else counter--;
202
+
203
+        if (counter == 0) msgRcv=msgTmp->Copy();
204
+        
205
+        delete(msgTmp);
206
+    } while (counter);
207
+    
208
+    delete (msg);
209
+    
210
+    return msgRcv;
147 211
 }
148 212
 
149 213
 /**
@@ -151,184 +215,142 @@ Message* ComRobot::Read() {
151 215
  * @param bytes Array of char
152 216
  * @return Message corresponding to received array of char
153 217
  */
154
-Message* ComRobot::CharToMessage(unsigned char *bytes) {
155
-    Message *msg = __null;
156
-    MessageFloat *msgf;
157
-    MessageBool *msgb;
158
-
159
-    switch (bytes[0]) {
160
-        case LABEL_ANGLE_POSITION:
161
-            msgf = new MessageFloat();
162
-            msgf->SetID(MESSAGE_ANGLE_POSITION);
163
-            msgf->SetValue(CharToFloat(&bytes[1]));
164
-            msg = (Message*) msgf;
218
+Message* ComRobot::StringToMessage(string s) {
219
+    Message *msg;
165 220
 
221
+    switch (s[0]) {
222
+        case LABEL_ROBOT_OK:
223
+            msg=new Message(MESSAGE_ANSWER_ACK);
166 224
             break;
167
-        case LABEL_ANGULAR_SPEED:
168
-            msgf = new MessageFloat();
169
-            msgf->SetID(MESSAGE_ANGULAR_SPEED);
170
-            msgf->SetValue(CharToFloat(&bytes[1]));
171
-            msg = (Message*) msgf;
172
-
225
+        case LABEL_ROBOT_ERROR:
226
+            msg=new Message(MESSAGE_ANSWER_ROBOT_ERROR);
173 227
             break;
174
-        case LABEL_BATTERY_LEVEL:
175
-            msgf = new MessageFloat();
176
-            msgf->SetID(MESSAGE_BATTERY);
177
-            msgf->SetValue(CharToFloat(&bytes[1]));
178
-            msg = (Message*) msgf;
179
-
228
+        case LABEL_ROBOT_UNKNOWN_COMMAND:
229
+            msg=new Message(MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND);
180 230
             break;
181
-        case LABEL_BETA_ANGLE:
182
-            msgf = new MessageFloat();
183
-            msgf->SetID(MESSAGE_BETA);
184
-            msgf->SetValue(CharToFloat(&bytes[1]));
185
-            msg = (Message*) msgf;
186
-
231
+        case '0':
232
+            msg=new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_EMPTY);
187 233
             break;
188
-        case LABEL_USER_PRESENCE:
189
-            msgb = new MessageBool();
190
-            msgb->SetID(MESSAGE_USER_PRESENCE);
191
-            msgb->SetState(CharToBool(&bytes[1]));
192
-            msg = (Message*) msgb;
193
-
234
+            case '1':
235
+            msg=new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_LOW);
236
+            break;
237
+            case '2':
238
+            msg=new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_FULL);
194 239
             break;
195 240
         default:
196
-            printf("Unknown message received from robot (%i)\n", bytes[0]);
197
-            fflush(stdout);
198
-            msg = new Message();
199
-    }
200
-
201
-    if (msg == NULL) {
202
-        printf("Message is null (%02X)\n", bytes[0]);
203
-        fflush(stdout);
204
-        msg = new Message();
241
+            msg=new Message(MESSAGE_ANSWER_ROBOT_ERROR);
242
+            cerr<<"["<<__PRETTY_FUNCTION__<<"] Unknown message received from robot (" << s <<")"<<endl<<flush;
205 243
     }
206 244
 
207 245
     return msg;
208 246
 }
209 247
 
210 248
 /**
211
- * Convert an array of char to float
212
- * @param bytes Array of char, containing a binary image of a float
213
- * @return Float value
214
- */
215
-float ComRobot::CharToFloat(unsigned char *bytes) {
216
-    unsigned long value;
217
-
218
-    union {
219
-        unsigned char buffer[4];
220
-        float f;
221
-    } convert;
222
-
223
-    convert.buffer[0] = bytes[0];
224
-    convert.buffer[1] = bytes[1];
225
-    convert.buffer[2] = bytes[2];
226
-    convert.buffer[3] = bytes[3];
227
-
228
-    //value = (bytes[3] << 24) | (bytes[2] << 16) | (bytes[1] << 8) | (bytes[0]);
229
-
230
-    return convert.f;
231
-}
232
-
233
-/**
234
- * Convert an array of char to integer
235
- * @param bytes Array of char, containing a binary image of an integer
236
- * @return Integer value
237
- */
238
-unsigned int ComRobot::CharToInt(unsigned char *bytes) {
239
-    unsigned long value;
240
-
241
-    value = (bytes[3] << 24) | (bytes[2] << 16) | (bytes[1] << 8) | (bytes[0]);
242
-
243
-    return (unsigned int) value;
244
-}
245
-
246
-/**
247
- * Convert an array of char to boolean
248
- * @param bytes Array of char, containing a binary image of a boolean
249
- * @return Boolean value
250
- */
251
-bool ComRobot::CharToBool(unsigned char *bytes) {
252
-    unsigned long value;
253
-
254
-    value = (bytes[3] << 24) | (bytes[2] << 16) | (bytes[1] << 8) | (bytes[0]);
255
-
256
-    if (value == 0) return false;
257
-
258
-    else return true;
259
-}
260
-
261
-/**
262
- * Send a message to robot
263
- * @param msg Message to send to robot
264
- * @return 1 if success, 0 otherwise
265
- * @attention Message is destroyed (delete) after being sent. You do not need to delete it yourself
266
- * @attention Write is blocking until message is written into buffer (linux side)
267
- * @warning Write is not thread save : check that multiple tasks can't access this method simultaneously  
268
- */
269
-int ComRobot::Write(Message* msg) {
270
-    unsigned char buffer[7];
271
-    int ret_val = 0;
272
-
273
-    MessageToChar(msg, buffer);
274
-
275
-    Write_Pre();
276
-
277
-    if (this->fd != -1) {
278
-        int count = write(this->fd, &buffer[0], 7); //Filestream, bytes to write, number of bytes to write
279
-        if (count < 0) {
280
-            printf("Warning: UART TX error in ComStm32::Write\n");
281
-        } else {
282
-            ret_val = 1;
283
-        }
284
-    }
285
-
286
-    // deallocation of msg
287
-    delete(msg);
288
-
289
-    Write_Post();
290
-
291
-    return ret_val;
292
-}
293
-
294
-/**
295 249
  * Convert a message to its array of char representation (for sending command to stm32)
296 250
  * @param msg Message to be sent to robot
297 251
  * @param buffer Array of char, image of message to send
298 252
  */
299
-void ComRobot::MessageToChar(Message *msg, unsigned char *buffer) {
253
+string ComRobot::MessageToString(Message *msg) {
254
+    string s;
255
+    
300 256
     float val_f;
301 257
     int val_i;
302 258
     unsigned char *b;
303 259
 
304
-    buffer[0] = '<';
305
-    buffer[6] = '\n';
306
-
307 260
     switch (msg->GetID()) {
308
-        case MESSAGE_TORQUE:
309
-            buffer[1] = LABEL_TORQUE;
310
-            val_f = ((MessageFloat*) msg)->GetValue();
311
-            b = (unsigned char *) &val_f;
312
-
261
+        case MESSAGE_ROBOT_PING:
262
+            s+=LABEL_ROBOT_PING;
313 263
             break;
314
-        case MESSAGE_EMERGENCY_STOP:
315
-            buffer[1] = LABEL_EMERGENCY_STOP;
316
-            if (((MessageBool*) msg)->GetState())
317
-                val_i = 1;
318
-            else
319
-                val_i = 0;
320
-            b = (unsigned char *) &val_i;
321
-
264
+        case MESSAGE_ROBOT_RESET:
265
+            s+=LABEL_ROBOT_RESET;
266
+            break;
267
+        case MESSAGE_ROBOT_POWEROFF:
268
+            s+=LABEL_ROBOT_POWEROFF;
269
+            break;
270
+        case MESSAGE_ROBOT_START_WITHOUT_WD:
271
+            s+=LABEL_ROBOT_START_WITHOUT_WD;
272
+            break;
273
+        case MESSAGE_ROBOT_START_WITH_WD:
274
+            s+=LABEL_ROBOT_START_WITH_WD;
275
+            break;
276
+        case MESSAGE_ROBOT_RELOAD_WD:
277
+            s+=LABEL_ROBOT_RELOAD_WD;
278
+            break;
279
+        case MESSAGE_ROBOT_BATTERY_GET:
280
+            s+=LABEL_ROBOT_GET_BATTERY;
281
+            break;
282
+        case MESSAGE_ROBOT_STATE_GET:
283
+            s+=LABEL_ROBOT_GET_STATE;
284
+            break;
285
+        case MESSAGE_ROBOT_GO_FORWARD:
286
+            s+=LABEL_ROBOT_MOVE;
287
+            s+=LABEL_ROBOT_SEPARATOR_CHAR;
288
+            s.append(to_string(500000));
289
+            break;
290
+        case MESSAGE_ROBOT_GO_BACKWARD:
291
+            s+=LABEL_ROBOT_MOVE;
292
+            s+=LABEL_ROBOT_SEPARATOR_CHAR;
293
+            s.append(to_string(-500000));
294
+            break;
295
+        case MESSAGE_ROBOT_GO_LEFT:
296
+            s+=LABEL_ROBOT_TURN;
297
+            s+=LABEL_ROBOT_SEPARATOR_CHAR;
298
+            s.append(to_string(90));
299
+            break;
300
+        case MESSAGE_ROBOT_GO_RIGHT:
301
+            s+=LABEL_ROBOT_TURN;
302
+            s+=LABEL_ROBOT_SEPARATOR_CHAR;
303
+            s.append(to_string(-90));
304
+            break;
305
+        case MESSAGE_ROBOT_MOVE:
306
+            s+=LABEL_ROBOT_MOVE;
307
+            s+=LABEL_ROBOT_SEPARATOR_CHAR;
308
+            s.append(to_string(((MessageInt*)msg)->GetValue()));
309
+            break;
310
+        case MESSAGE_ROBOT_TURN:
311
+            s+=LABEL_ROBOT_TURN;
312
+            s+=LABEL_ROBOT_SEPARATOR_CHAR;
313
+            s.append(to_string(((MessageInt*)msg)->GetValue()));
322 314
             break;
323 315
         default:
324
-            printf("Invalid message to send");
325
-            val_i = 0;
326
-            b = (unsigned char *) &val_i;
316
+            cerr<<"["<<__PRETTY_FUNCTION__<<"] Invalid message for robot ("<<msg->ToString()<<")"<<endl<<flush;
317
+            throw std::runtime_error {"Invalid message"};
327 318
     }
328 319
 
329
-    buffer[2] = b[0];
330
-    buffer[3] = b[1];
331
-    buffer[4] = b[2];
332
-    buffer[5] = b[3];
320
+    return s;
333 321
 }
334 322
 
323
+/**
324
+ * Add a checksum and carriage return to a command string
325
+ * @param[in,out] s String containing command for robot, without ending char (carriage return) 
326
+ */
327
+void ComRobot::AddChecksum(string &s) {
328
+    unsigned char checksum=0;
329
+    
330
+    for (string::iterator it=s.begin(); it!=s.end(); ++it) {
331
+        checksum ^= (unsigned char)*it;
332
+    }
333
+    
334
+    s+=(char)checksum; // Add calculated checksum
335
+    s+=(char)LABEL_ROBOT_ENDING_CHAR;
336
+}
337
+
338
+/**
339
+ * Verify if checksum of an incoming answer from robot is valid, 
340
+ * then remove checksum from incoming answer (if checksum is ok)
341
+ * @param[in,out] s String containing incoming answer from robot
342
+ * @return true is checksum is valid, false otherwise.
343
+ */
344
+bool ComRobot::VerifyChecksum(string &s) {
345
+    unsigned char checksum=0;
346
+    
347
+    for (string::iterator it=s.begin(); it!=s.end(); ++it) {
348
+        checksum ^= (unsigned char)*it;
349
+    }
350
+    
351
+    if (checksum==0) { // checksum is ok, remove last char of string (checksum)
352
+        s.pop_back(); // remove last char
353
+        return true;
354
+    }
355
+    else return false;
356
+}

+ 115
- 60
software/raspberry/superviseur-robot/lib/comrobot.h View File

@@ -19,6 +19,7 @@
19 19
 #define __COMROBOT_H__
20 20
 
21 21
 #include "messages.h"
22
+#include <string>
22 23
 
23 24
 using namespace std;
24 25
 
@@ -30,114 +31,168 @@ using namespace std;
30 31
  */
31 32
 class ComRobot {
32 33
 public:
34
+
33 35
     /**
34 36
      * Constructor
35 37
      */
36
-    ComRobot() {}
37
-    
38
+    ComRobot() {
39
+    }
40
+
38 41
     /**
39 42
      * Destructor
40 43
      */
41
-    virtual ~ComRobot() {}
42
-    
44
+    virtual ~ComRobot() {
45
+    }
46
+
43 47
     /**
44 48
      * Open serial link with robot
45 49
      * @return File descriptor
46 50
      * @throw std::runtime_error if it fails
47 51
      */
48 52
     int Open();
49
-    
53
+
54
+    /**
55
+     * Open serial link with robot
56
+     * @param usart Filename of usart to open
57
+     * @return File descriptor
58
+     * @throw std::runtime_error if it fails
59
+     */
60
+    int Open(string usart);
61
+
50 62
     /**
51 63
      * Close serial link
52 64
      * @return Success if above 0, failure if below 0
53 65
      */
54 66
     int Close();
55
-    
56
-    /**
57
-     * Get a message from robot
58
-     * @return Message currently received
59
-     * @attention A message object is created (new) when receiving data from robot. You MUST remember to destroy is (delete) after use
60
-     * @attention Read method is blocking until a message is received
61
-     * @warning Read is not thread safe : Do not call it in multiple tasks simultaneously
62
-     */
63
-    Message* Read();
64
-    
67
+
65 68
     /**
66 69
      * Send a message to robot
67 70
      * @param msg Message to send to robot
68
-     * @return 1 if success, 0 otherwise
69
-     * @attention Message is destroyed (delete) after being sent. You do not need to delete it yourself
71
+     * @return A message containing either an answer (Ack/Nak/Timeout/Error) or a value (battery level, robot state) depending of the command
72
+     * @attention Input message is destroyed (delete) after being sent. You do not need to delete it yourself
73
+     * @attention Write produce an answer message. You have to dispose it (delete) when you have finished using it
70 74
      * @attention Write is blocking until message is written into buffer (linux side)
71 75
      * @warning Write is not thread save : check that multiple tasks can't access this method simultaneously  
72 76
      */
73
-    int Write(Message* msg);
74
-    
75
-    /**
76
-     * Function called at beginning of Read method
77
-     * Use it to do some synchronization (call of mutex, for example)
78
-     */
79
-    virtual void Read_Pre() {}
80
-    
81
-    /**
82
-     * Function called at end of Read method
83
-     * Use it to do some synchronization (call of mutex, for example)
84
-     */
85
-    virtual void Read_Post() {}
86
-    
77
+    Message *Write(Message* msg);
78
+
87 79
     /**
88 80
      * Function called at beginning of Write method
89 81
      * Use it to do some synchronization (call of mutex, for example)
90 82
      */
91
-    virtual void Write_Pre() {}
92
-    
83
+    virtual void Write_Pre() {
84
+    }
85
+
93 86
     /**
94 87
      * Function called at end of Write method
95 88
      * Use it to do some synchronization (call of mutex, for example)
96 89
      */
97
-    virtual void Write_Post() {}
90
+    virtual void Write_Post() {
91
+    }
98 92
     
99
-    static Message *Ping();
93
+    Message *SendCommand(Message* msg, MessageID answerID, int maxRetries);
100 94
     
95
+    static Message *Ping() {
96
+        return new Message(MESSAGE_ROBOT_PING);
97
+    }
98
+
99
+    static Message *Reset() {
100
+        return new Message(MESSAGE_ROBOT_RESET);
101
+    }
102
+
103
+    static Message *PowerOff() {
104
+        return new Message(MESSAGE_ROBOT_POWEROFF);
105
+    }
106
+
107
+    static Message *StartWithWD() {
108
+        return new Message(MESSAGE_ROBOT_START_WITH_WD);
109
+    }
110
+
111
+    static Message *StartWithoutWD() {
112
+        return new Message(MESSAGE_ROBOT_START_WITHOUT_WD);
113
+    }
114
+
115
+    static Message *ReloadWD() {
116
+        return new Message(MESSAGE_ROBOT_RELOAD_WD);
117
+    }
118
+
119
+    static Message *Move(int length) {
120
+        return new MessageInt(MESSAGE_ROBOT_MOVE, length);
121
+    }
122
+
123
+    static Message *Turn(int angle) {
124
+        return new MessageInt(MESSAGE_ROBOT_TURN, angle);
125
+    }
126
+
127
+    static Message *Stop() {
128
+        return new Message(MESSAGE_ROBOT_STOP);
129
+    }
130
+
131
+    static Message *GoForward() {
132
+        return new Message(MESSAGE_ROBOT_GO_FORWARD);
133
+    }
134
+
135
+    static Message *GoBackward() {
136
+        return new Message(MESSAGE_ROBOT_GO_BACKWARD);
137
+    }
138
+
139
+    static Message *GoLeft() {
140
+        return new Message(MESSAGE_ROBOT_GO_LEFT);
141
+    }
142
+
143
+    static Message *GoRight() {
144
+        return new Message(MESSAGE_ROBOT_GO_RIGHT);
145
+    }
146
+
147
+    static Message *GetBattery() {
148
+        return new Message(MESSAGE_ROBOT_BATTERY_GET);
149
+    }
150
+
151
+    static Message *GetState() {
152
+        return new Message(MESSAGE_ROBOT_STATE_GET);
153
+    }
154
+
101 155
 protected:
102 156
     /**
103 157
      * Serial link file descriptor
104 158
      */
105 159
     int fd;
106
-    
160
+
107 161
     /**
108
-     * Convert an array of char to float
109
-     * @param bytes Array of char, containing a binary image of a float
110
-     * @return Float value
162
+     * Get an answer from robot
163
+     * @return String containing answer from robot
164
+     * @attention Read method is blocking until a message is received (timeout of 500 ms)
165
+     * @warning Read is not thread safe : Do not call it in multiple tasks simultaneously
111 166
      */
112
-    float CharToFloat(unsigned char *bytes);
113
-    
167
+    string Read();
168
+
114 169
     /**
115
-     * Convert an array of char to boolean
116
-     * @param bytes Array of char, containing a binary image of a boolean
117
-     * @return Boolean value
170
+     * Convert a string to its message representation (when receiving data from robot)
171
+     * @param s String from robot containing answer
172
+     * @return Message corresponding to received array of char
118 173
      */
119
-    bool CharToBool(unsigned char *bytes);
120
-    
174
+    Message* StringToMessage(string s);
175
+
121 176
     /**
122
-     * Convert an array of char to integer
123
-     * @param bytes Array of char, containing a binary image of an integer
124
-     * @return Integer value
177
+     * Convert a message to its string representation (for sending command to robot)
178
+     * @param msg Message to be sent to robot
179
+     * @return String containing command to robot
125 180
      */
126
-    unsigned int CharToInt(unsigned char *bytes);
127
-    
181
+    string MessageToString(Message *msg);
182
+
128 183
     /**
129
-     * Convert an array of char to its message representation (when receiving data from stm32)
130
-     * @param bytes Array of char
131
-     * @return Message corresponding to received array of char
184
+     * Add a checksum and carriage return to a command string
185
+     * @param[in,out] s String containing command for robot, without ending char (carriage return) 
132 186
      */
133
-    Message* CharToMessage(unsigned char *bytes);
134
-    
187
+    void AddChecksum(string &s);
188
+
135 189
     /**
136
-     * Convert a message to its array of char representation (for sending command to stm32)
137
-     * @param msg Message to be sent to robot
138
-     * @param buffer Array of char, image of message to send
190
+     * Verify if checksum of an incoming answer from robot is valid, 
191
+     * then remove checksum from incoming answer (if checksum is ok)
192
+     * @param[in,out] s String containing incoming answer from robot
193
+     * @return true is checksum is valid, false otherwise.
139 194
      */
140
-    void MessageToChar(Message *msg, unsigned char *buffer);
195
+    bool VerifyChecksum(string &s);
141 196
 };
142 197
 
143 198
 #endif /* __COMROBOT_H__ */

+ 12
- 8
software/raspberry/superviseur-robot/lib/img.cpp View File

@@ -27,9 +27,13 @@ Img::Img(ImageMat imgMatrice) {
27 27
 }
28 28
 
29 29
 string Img::ToString() {
30
-    return "Image size: "+this->img.cols+"x"this->img.rows+" (dim="+this->img.dims+")";
30
+    return "Image size: "+to_string(this->img.cols)+"x"+to_string(this->img.rows)+" (dim="+to_string(this->img.dims)+")";
31 31
 }
32 32
     
33
+string Img::ToBase64() {
34
+    return "";
35
+}
36
+
33 37
 Img* Img::Copy() {
34 38
     return new Img(this->img);
35 39
 }
@@ -100,13 +104,13 @@ Jpg Img::toJpg() {
100 104
     return imgJpg;
101 105
 }
102 106
 
103
-string Img::ToBase64() {
104
-    string imgBase64;
105
-    Jpg imgJpg = toJpg();
106
-    
107
-    /* faire la convertion Jpg vers base 64 */
108
-    return imgBase64;
109
-}
107
+//string Img::ToBase64() {
108
+//    string imgBase64;
109
+//    Jpg imgJpg = toJpg();
110
+//    
111
+//    /* faire la convertion Jpg vers base 64 */
112
+//    return imgBase64;
113
+//}
110 114
 
111 115
 std::list<Position> Img::search_robot(Arene monArene) {
112 116
 

+ 1
- 1
software/raspberry/superviseur-robot/lib/img.h View File

@@ -49,7 +49,7 @@ typedef struct {
49 49
 
50 50
 class Arene {
51 51
 public:
52
-    Arene();
52
+    Arene() {}
53 53
     
54 54
     cv::Rect arene;
55 55
     bool empty();

+ 26
- 205
software/raspberry/superviseur-robot/lib/messages.cpp View File

@@ -27,7 +27,12 @@
27 27
 const string MESSAGE_ID_STRING[] = {
28 28
     "Empty",
29 29
     "Log",
30
-    "Answer",
30
+    "Answer [Acknowledge]",
31
+    "Answer [Not Acknowledge]",
32
+    "Answer [Command timeout]",
33
+    "Answer [Command unknown]",
34
+    "Answer [Command error]",
35
+    "Answer [Communication error]",
31 36
     "Monitor connection lost",
32 37
     "Open serial com",
33 38
     "Close serial com",
@@ -42,8 +47,8 @@ const string MESSAGE_ID_STRING[] = {
42 47
     "Image",
43 48
     "Robot ping",
44 49
     "Robot reset",
45
-    "Robot start with wtachdog",
46
-    "Robot start without wtachdog",
50
+    "Robot start with watchdog",
51
+    "Robot start without watchdog",
47 52
     "Robot reload watchdog",
48 53
     "Robot move",
49 54
     "Robot turn",
@@ -56,7 +61,9 @@ const string MESSAGE_ID_STRING[] = {
56 61
     "Robot get battery",
57 62
     "Robot battery level",
58 63
     "Robot get state",
59
-    "Robot current state"
64
+    "Robot current state",
65
+    "Robot state [Not busy]",
66
+    "Robot state [Busy]"
60 67
 };
61 68
 
62 69
 /*
@@ -108,7 +115,7 @@ void Message::SetID(MessageID id) {
108 115
  */
109 116
 string Message::ToString() {
110 117
     if (CheckID(this->messageID))
111
-        return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"";
118
+        return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"";
112 119
     else
113 120
         return "Invalid message";
114 121
 }
@@ -118,7 +125,7 @@ string Message::ToString() {
118 125
  * @return A message, copy of current
119 126
  */
120 127
 Message* Message::Copy() {
121
-    Message *msg = new Message();
128
+    Message *msg = new Message(this->messageID);
122 129
 
123 130
     return msg;
124 131
 }
@@ -128,30 +135,12 @@ Message* Message::Copy() {
128 135
  * @return Current message ID
129 136
  */
130 137
 bool Message::CheckID(MessageID id) {
131
-    if ((id != MESSAGE_EMPTY) &&
132
-            (id != MESSAGE_MONITOR_LOST) &&
133
-            (id != MESSAGE_ARENA_CONFIRM) &&
134
-            (id != MESSAGE_ARENA_INFIRM) &&
135
-            (id != MESSAGE_ASK_ARENA) &&
136
-            (id != MESSAGE_CAM_CLOSE) &&
137
-            (id != MESSAGE_CAM_OPEN) &&
138
-            (id != MESSAGE_CLOSE_COM) &&
139
-            (id != MESSAGE_COMPUTE_POSITION) &&
140
-            (id != MESSAGE_OPEN_COM) &&
141
-            (id != MESSAGE_ROBOT_GET_BATTERY) &&
142
-            (id != MESSAGE_ROBOT_GET_STATE) &&
143
-            (id != MESSAGE_ROBOT_GO_BACK) &&
144
-            (id != MESSAGE_ROBOT_GO_FORWARD) &&
145
-            (id != MESSAGE_ROBOT_GO_LEFT) &&
146
-            (id != MESSAGE_ROBOT_GO_RIGHT) &&
147
-            (id != MESSAGE_ROBOT_PING) &&
148
-            (id != MESSAGE_ROBOT_POWEROFF) &&
149
-            (id != MESSAGE_ROBOT_RELOAD_WD) &&
150
-            (id != MESSAGE_ROBOT_RESET) &&
151
-            (id != MESSAGE_ROBOT_START_WITHOUT_WD) &&
152
-            (id != MESSAGE_ROBOT_START_WITH_WD) &&
153
-            (id != MESSAGE_ROBOT_STOP) &&
154
-            (id != MESSAGE_STOP_COMPUTE_POSITION)) {
138
+    if ((id == MESSAGE_CAM_IMAGE) ||
139
+        (id == MESSAGE_CAM_POSITION) ||
140
+        (id == MESSAGE_ROBOT_MOVE) ||
141
+        (id == MESSAGE_ROBOT_TURN) ||
142
+        (id == MESSAGE_LOG) ||
143
+        (id == MESSAGE_ROBOT_BATTERY_LEVEL)) {
155 144
         return false;
156 145
     } else return true;
157 146
 }
@@ -197,7 +186,7 @@ void MessageInt::SetID(MessageID id) {
197 186
  */
198 187
 string MessageInt::ToString() {
199 188
     if (CheckID(this->messageID))
200
-        return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nValue: " + to_string(this->value);
189
+        return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nValue: " + to_string(this->value);
201 190
     else
202 191
         return "Invalid message";
203 192
 }
@@ -263,7 +252,7 @@ void MessageString::SetID(MessageID id) {
263 252
  */
264 253
 string MessageString::ToString() {
265 254
     if (CheckID(this->messageID))
266
-        return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nString: \"" + this->s + "\"";
255
+        return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nString: \"" + this->s + "\"";
267 256
     else
268 257
         return "Invalid message";
269 258
 }
@@ -342,7 +331,7 @@ void MessageImg::SetImage(Img* image) {
342 331
  */
343 332
 string MessageImg::ToString() {
344 333
     if (CheckID(this->messageID))
345
-        return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\n" + this->image->ToString();
334
+        return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\n" + this->image->ToString();
346 335
     else
347 336
         return "Invalid message";
348 337
 }
@@ -362,91 +351,7 @@ Message* MessageImg::Copy() {
362 351
  * @return true, if message ID is acceptable, false otherwise
363 352
  */
364 353
 bool MessageImg::CheckID(MessageID id) {
365
-    if (id != MESSAGE_IMAGE) {
366
-        return false;
367
-    } else return true;
368
-}
369
-
370
-/* class MessageAnswer*/
371
-
372
-/**
373
- * Create a new, empty answer message
374
- */
375
-MessageAnswer::MessageAnswer() {
376
-    answer=ANSWER_ACK;
377
-}
378
-
379
-/**
380
- * Create a new answer message, with given ID and answer
381
- * @param id Message ID
382
- * @param ans Answer ID
383
- * @throw std::runtime_error if message ID is incompatible with string data
384
- */
385
-MessageAnswer::MessageAnswer(MessageID id, AnswerID ans) {
386
-    MessageAnswer::SetID(id);
387
-    MessageAnswer::SetAnswer(ans);
388
-}
389
-
390
-/**
391
- * Set message ID
392
- * @param id Message ID
393
- * @throw std::runtime_error if message ID is incompatible with answer message
394
- */
395
-void MessageAnswer::SetID(MessageID id) {
396
-    if (CheckID(id))
397
-        messageID = id;
398
-    else
399
-        throw std::runtime_error {
400
-        "Invalid message id for MessageAnswer"
401
-    };
402
-}
403
-
404
-/**
405
- * Set message answer
406
- * @param ans Answer ID
407
- * @throw std::runtime_error if answer ID is incompatible with answer data
408
- */
409
-void MessageAnswer::SetAnswer(AnswerID ans) {
410
-    if ((ans != ANSWER_ACK) &&
411
-            (ans != ANSWER_NACK) &&
412
-            (ans != ANSWER_LOST_ROBOT) &&
413
-            (ans != ANSWER_ROBOT_CHECKSUM) &&
414
-            (ans != ANSWER_ROBOT_ERROR) &&
415
-            (ans != ANSWER_ROBOT_TIMEOUT) &&
416
-            (ans != ANSWER_ROBOT_UNKNOWN_COMMAND)) {
417
-        this->answer = answer;
418
-    } else {
419
-        throw std::runtime_error{
420
-            "Invalid answer for MessageAnswer"};
421
-    }
422
-}
423
-
424
-/**
425
- * Translate content of message into a string that can be displayed
426
- * @return A string describing message contents
427
- */
428
-string MessageAnswer::ToString() {
429
-    if (CheckID(this->messageID))
430
-        return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nAnswer: \"" + ANSWER_ID_STRING[this->answer] + "\"";
431
-    else
432
-        return "Invalid message";
433
-}
434
-
435
-/**
436
- * Allocate a new message and copy contents of current message
437
- * @return A message, copy of current
438
- */
439
-Message* MessageAnswer::Copy() {
440
-    return new MessageAnswer(this->messageID, this->answer);
441
-}
442
-
443
-/**
444
- * Verify if message ID is compatible with current message type
445
- * @param id Message ID
446
- * @return true, if message ID is acceptable, false otherwise
447
- */
448
-bool MessageAnswer::CheckID(MessageID id) {
449
-    if ((id != MESSAGE_ANSWER)) {
354
+    if (id != MESSAGE_CAM_IMAGE) {
450 355
         return false;
451 356
     } else return true;
452 357
 }
@@ -523,7 +428,7 @@ string MessageBattery::ToString() {
523 428
     }
524 429
     
525 430
     if (CheckID(this->messageID))
526
-        return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nBattery level: \"" + levelString + "\"";
431
+        return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nBattery level: \"" + levelString + "\"";
527 432
     else
528 433
         return "Invalid message";
529 434
 }
@@ -603,7 +508,7 @@ void MessagePosition::SetPosition(Position& pos) {
603 508
  */
604 509
 string MessagePosition::ToString() {
605 510
     if (CheckID(this->messageID))
606
-        return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nPosition: \"" + to_string(this->pos.center.x) + ";" + to_string(this->pos.center.y) + "\"";
511
+        return "Message: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nPosition: \"" + to_string(this->pos.center.x) + ";" + to_string(this->pos.center.y) + "\"";
607 512
     else
608 513
         return "Invalid message";
609 514
 }
@@ -622,91 +527,7 @@ Message* MessagePosition::Copy() {
622 527
  * @return true, if message ID is acceptable, false otherwise
623 528
  */
624 529
 bool MessagePosition::CheckID(MessageID id) {
625
-    if ((id != MESSAGE_POSITION)) {
530
+    if ((id != MESSAGE_CAM_POSITION)) {
626 531
         return false;
627 532
     } else return true;
628 533
 }
629
-
630
-
631
-/* class MessageState */
632
-
633
-/**
634
- * Create a new, empty state message
635
- */
636
-MessageState::MessageState() {
637
-    state = ROBOT_NOT_BUSY;
638
-}
639
-
640
-/**
641
- * Create a new string message, with given ID and string
642
- * @param id Message ID
643
- * @param s Message string
644
- * @throw std::runtime_error if message ID is incompatible with string data
645
- */
646
-MessageState::MessageState(MessageID id, RobotState state) {
647
-    MessageState::SetID(id);
648
-    MessageState::SetState(state);
649
-}
650
-
651
-/**
652
- * Set message ID
653
- * @param id Message ID
654
- * @throw std::runtime_error if message ID is incompatible with robot state
655
- */
656
-void MessageState::SetID(MessageID id) {
657
-    if (CheckID(id))
658
-        messageID = id;
659
-    else
660
-        throw std::runtime_error {
661
-        "Invalid message id for MessageState"
662
-    };
663
-}
664
-
665
-/**
666
- * Set robot state
667
- * @param state Robot state
668
- */
669
-void MessageState::SetState(RobotState state) {
670
-    if ((state != ROBOT_NOT_BUSY) && (state != ROBOT_BUSY)) {
671
-        throw std::runtime_error{
672
-            "Invalid state for MessageState"};
673
-    } else {
674
-        this->state = state;
675
-    }
676
-}
677
-    
678
-/**
679
- * Translate content of message into a string that can be displayed
680
- * @return A string describing message contents
681
- */
682
-string MessageState::ToString() {
683
-    string stateString;
684
-    
685
-    if (this->state == ROBOT_NOT_BUSY) stateString="Not busy";
686
-    else if (this->state == ROBOT_BUSY) stateString="Busy";
687
-    else stateString="Invalid state";
688
-    
689
-    if (CheckID(this->messageID))
690
-        return "Id: \"" + MESSAGE_ID_STRING[this->messageID] + "\"\nState: \"" + stateString + "\"";
691
-    else
692
-        return "Invalid message";
693
-}
694
-
695
-/**
696
- * Allocate a new message and copy contents of current message
697
- * @return A message, copy of current
698
- */
699
-Message* MessageState::Copy() {
700
-    return new MessageState(this->messageID, this->state);
701
-}
702
-
703
-/**
704
- * Verify if message ID is compatible with current message type
705
- * @param id Message ID
706
- * @return true, if message ID is acceptable, false otherwise
707
- */
708
-bool MessageState::CheckID(MessageID id) {
709
-    if ((id != MESSAGE_ROBOT_CURRENT_STATE)) {
710
-        return false;
711
-    } else return true;
712
-}

+ 29
- 164
software/raspberry/superviseur-robot/lib/messages.h View File

@@ -33,11 +33,16 @@ typedef enum {
33 33
     MESSAGE_LOG,
34 34
     
35 35
     // Message containing answer (after robot command, or for monitor)
36
-    MESSAGE_ANSWER,
36
+    MESSAGE_ANSWER_ACK,
37
+    MESSAGE_ANSWER_NACK,
38
+    MESSAGE_ANSWER_ROBOT_TIMEOUT,
39
+    MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND,
40
+    MESSAGE_ANSWER_ROBOT_ERROR,
41
+    MESSAGE_ANSWER_COM_ERROR,
37 42
            
38 43
     // messages for serial communication with robot
39
-    MESSAGE_OPEN_COM,
40
-    MESSAGE_CLOSE_COM,
44
+    MESSAGE_ROBOT_COM_OPEN,
45
+    MESSAGE_ROBOT_COM_CLOSE,
41 46
          
42 47
     // Messages specific to server
43 48
     MESSAGE_MONITOR_LOST,
@@ -45,13 +50,13 @@ typedef enum {
45 50
     // Messages for camera   
46 51
     MESSAGE_CAM_OPEN,
47 52
     MESSAGE_CAM_CLOSE,
48
-    MESSAGE_ASK_ARENA,
49
-    MESSAGE_ARENA_CONFIRM,
50
-    MESSAGE_ARENA_INFIRM,
51
-    MESSAGE_COMPUTE_POSITION,
52
-    MESSAGE_STOP_COMPUTE_POSITION,
53
-    MESSAGE_POSITION,
54
-    MESSAGE_IMAGE,
53
+    MESSAGE_CAM_ASK_ARENA,
54
+    MESSAGE_CAM_ARENA_CONFIRM,
55
+    MESSAGE_CAM_ARENA_INFIRM,
56
+    MESSAGE_CAM_POSITION_COMPUTE_START,
57
+    MESSAGE_CAM_POSITION_COMPUTE_STOP,
58
+    MESSAGE_CAM_POSITION,
59
+    MESSAGE_CAM_IMAGE,
55 60
             
56 61
     // Messages for robot
57 62
     MESSAGE_ROBOT_PING,
@@ -62,39 +67,25 @@ typedef enum {
62 67
     MESSAGE_ROBOT_MOVE,
63 68
     MESSAGE_ROBOT_TURN,
64 69
     MESSAGE_ROBOT_GO_FORWARD,
65
-    MESSAGE_ROBOT_GO_BACK,
70
+    MESSAGE_ROBOT_GO_BACKWARD,
66 71
     MESSAGE_ROBOT_GO_LEFT,
67 72
     MESSAGE_ROBOT_GO_RIGHT,
68 73
     MESSAGE_ROBOT_STOP,
69 74
     MESSAGE_ROBOT_POWEROFF,
70
-    MESSAGE_ROBOT_GET_BATTERY,
75
+    MESSAGE_ROBOT_BATTERY_GET,
71 76
     MESSAGE_ROBOT_BATTERY_LEVEL,
72
-    MESSAGE_ROBOT_GET_STATE,
73
-    MESSAGE_ROBOT_CURRENT_STATE
77
+    MESSAGE_ROBOT_STATE_GET,
78
+    MESSAGE_ROBOT_STATE_NOT_BUSY,
79
+    MESSAGE_ROBOT_STATE_BUSY
74 80
 } MessageID;
75 81
 
76 82
 typedef enum {
77
-    ANSWER_ACK=0,
78
-    ANSWER_NACK,
79
-    ANSWER_LOST_ROBOT,
80
-    ANSWER_ROBOT_TIMEOUT,
81
-    ANSWER_ROBOT_UNKNOWN_COMMAND,
82
-    ANSWER_ROBOT_ERROR,
83
-    ANSWER_ROBOT_CHECKSUM
84
-} AnswerID;
85
-
86
-typedef enum {
87 83
     BATTERY_UNKNOWN=-1,
88 84
     BATTERY_EMPTY=0,
89 85
     BATTERY_LOW,
90 86
     BATTERY_FULL
91 87
 } BatteryLevel;
92 88
 
93
-typedef enum {
94
-    ROBOT_NOT_BUSY=0,
95
-    ROBOT_BUSY
96
-} RobotState;
97
-
98 89
 using namespace std;
99 90
 
100 91
 /**
@@ -133,6 +124,15 @@ public:
133 124
     virtual Message* Copy();
134 125
 
135 126
     /**
127
+     * Compare message ID
128
+     * @param id Id to compare message to
129
+     * @return true if id is equal to message id, false otherwise
130
+     */
131
+    bool CompareID(MessageID id) {
132
+        return (this->messageID == id) ? true:false;
133
+    }
134
+    
135
+    /**
136 136
      * Get message ID
137 137
      * @return Current message ID
138 138
      */
@@ -563,140 +563,5 @@ protected:
563 563
     bool CheckID(MessageID id);
564 564
 };
565 565
 
566
-/**
567
- * Message class for holding answer, based on Message class
568
- * 
569
- * @brief Answer message class
570
- * 
571
- */
572
-class MessageAnswer : public Message {
573
-public:
574
-    /**
575
-     * Create a new, empty image message
576
-     */
577
-    MessageAnswer();
578
-
579
-    /**
580
-     * Create a new image message, with given ID and boolean value
581
-     * @param id Message ID
582
-     * @param ans Answer ID
583
-     * @throw std::runtime_error if message ID is incompatible with image message
584
-     */
585
-    MessageAnswer(MessageID id, AnswerID ans);
586
-
587
-    /**
588
-     * Set message ID
589
-     * @param id Message ID
590
-     * @throw std::runtime_error if message ID is incompatible withimage message
591
-     */
592
-    void SetID(MessageID id);
593
-
594
-    /**
595
-     * Get message image
596
-     * @return Pointer to image
597
-     */
598
-    AnswerID GetAnswer() {
599
-        return answer;
600
-    }
601
-
602
-    /**
603
-     * Set message answer
604
-     * @param ans Answer ID
605
-     */
606
-    void SetAnswer(AnswerID ans);
607
-
608
-    /**
609
-     * Translate content of message into a string that can be displayed
610
-     * @return A string describing message contents
611
-     */
612
-    string ToString();
613
-
614
-    /**
615
-     * Allocate a new message and copy contents of current message
616
-     * @return A message, copy of current
617
-     */
618
-    Message* Copy();
619
-
620
-protected:
621
-    /**
622
-     * Message answer
623
-     */
624
-   AnswerID answer;
625
-
626
-    /**
627
-     * Verify if message ID is compatible with current message type
628
-     * @param id Message ID
629
-     * @return true, if message ID is acceptable, false otherwise
630
-     */
631
-    bool CheckID(MessageID id);
632
-};
633
-
634
-/**
635
- * Message class for holding robot state, based on Message class
636
- * 
637
- * @brief Answer message class
638
- * 
639
- */
640
-class MessageState: public Message {
641
-public:
642
-    /**
643
-     * Create a new, empty image message
644
-     */
645
-    MessageState();
646
-
647
-    /**
648
-     * Create a new image message, with given ID and boolean value
649
-     * @param id Message ID
650
-     * @param image Pointer to image
651
-     * @throw std::runtime_error if message ID is incompatible with image message
652
-     */
653
-    MessageState(MessageID id, RobotState state);
654
-
655
-    /**
656
-     * Set message ID
657
-     * @param id Message ID
658
-     * @throw std::runtime_error if message ID is incompatible withimage message
659
-     */
660
-    void SetID(MessageID id);
661
-
662
-    /**
663
-     * Get message image
664
-     * @return Pointer to image
665
-     */
666
-    RobotState GetState() {
667
-        return state;
668
-    }
669
-
670
-    /**
671
-     * Set message image
672
-     * @param image Pointer to image object
673
-     */
674
-    void SetState(RobotState state);
675
-
676
-    /**
677
-     * Translate content of message into a string that can be displayed
678
-     * @return A string describing message contents
679
-     */
680
-    string ToString();
681
-
682
-    /**
683
-     * Allocate a new message and copy contents of current message
684
-     * @return A message, copy of current
685
-     */
686
-    Message* Copy();
687
-
688
-protected:
689
-    /**
690
-     * Robot state
691
-     */
692
-   RobotState state;
693
-
694
-    /**
695
-     * Verify if message ID is compatible with current message type
696
-     * @param id Message ID
697
-     * @return true, if message ID is acceptable, false otherwise
698
-     */
699
-    bool CheckID(MessageID id);
700
-};
701 566
 #endif /* __MESSAGES_H__ */
702 567
 

+ 21
- 178
software/raspberry/superviseur-robot/main.cpp View File

@@ -15,196 +15,39 @@
15 15
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
16 16
  */
17 17
 
18
-/**
19
- * \file      main.cpp
20
- * \author    PE.Hladik
21
- * \version   1.0
22
- * \date      06/06/2017
23
- * \brief     main program
24
- */
25
-
26
-#include <stdio.h>
27
-#include <stdlib.h>
18
+#include <iostream>
28 19
 #include <unistd.h>
29 20
 
30 21
 #include <sys/mman.h>
31
-#include <alchemy/task.h>
32
-#include <alchemy/timer.h>
33
-#include <alchemy/mutex.h>
34
-#include <alchemy/sem.h>
35
-#include <alchemy/queue.h>
36 22
 
23
+#ifdef __WITH_PTHREAD__
24
+#include "tasks_pthread.h"
25
+#else
37 26
 #include "tasks.h"
38
-
39
-// Déclaration des taches
40
-RT_TASK th_server;
41
-RT_TASK th_sendToMon;
42
-RT_TASK th_receiveFromMon;
43
-RT_TASK th_openComRobot;
44
-RT_TASK th_startRobot;
45
-RT_TASK th_move;
46
-
47
-// Déclaration des priorités des taches
48
-int PRIORITY_TSERVER = 30;
49
-int PRIORITY_TOPENCOMROBOT = 20;
50
-int PRIORITY_TMOVE = 10;
51
-int PRIORITY_TSENDTOMON = 25;
52
-int PRIORITY_TRECEIVEFROMMON = 22;
53
-int PRIORITY_TSTARTROBOT = 20;
54
-
55
-RT_MUTEX mutex_robotStarted;
56
-RT_MUTEX mutex_move;
57
-
58
-// Déclaration des sémaphores
59
-RT_SEM sem_barrier;
60
-RT_SEM sem_openComRobot;
61
-RT_SEM sem_serverOk;
62
-RT_SEM sem_startRobot;
63
-
64
-// Déclaration des files de message
65
-RT_QUEUE q_messageToMon;
66
-
67
-int MSG_QUEUE_SIZE = 10;
68
-
69
-// Déclaration des ressources partagées
70
-int etatCommMoniteur = 1;
71
-int robotStarted = 0;
72
-char robotMove = DMB_STOP_MOVE;
73
-
74
-/**
75
- * \fn void initStruct(void)
76
- * \brief Initialisation des structures de l'application (tâches, mutex, 
77
- * semaphore, etc.)
78
- */
79
-void initStruct(void);
80
-
81
-/**
82
- * \fn void startTasks(void)
83
- * \brief Démarrage des tâches
84
- */
85
-void startTasks(void);
86
-
87
-/**
88
- * \fn void deleteTasks(void)
89
- * \brief Arrêt des tâches
90
- */
91
-void deleteTasks(void);
27
+#endif // __WITH_PTHREAD__
92 28
 
93 29
 int main(int argc, char **argv) {
94
-    int err;
30
+    Tasks tasks;
31
+    
95 32
     //Lock the memory to avoid memory swapping for this program
96 33
     mlockall(MCL_CURRENT | MCL_FUTURE);
97 34
 
98
-    printf("#################################\n");
99
-    printf("#      DE STIJL PROJECT         #\n");
100
-    printf("#################################\n");
101
-
102
-    initStruct();
103
-    startTasks();
104
-    rt_sem_broadcast(&sem_barrier);
105
-    pause();
106
-    deleteTasks();
107
-
108
-    return 0;
109
-}
110
-
111
-void initStruct(void) {
35
+    cout<<"#################################"<<endl;
36
+    cout<<"#      DE STIJL PROJECT         #"<<endl;
37
+    cout<<"#################################"<<endl;
112 38
 
113
-    int err;
114
-    /* Creation des mutex */
115
-    if (err = rt_mutex_create(&mutex_robotStarted, NULL)) {
116
-        printf("Error mutex create: %d %s\n", err, strerror(-err));
117
-        exit(EXIT_FAILURE);
118
-    }
119
-    if (err = rt_mutex_create(&mutex_move, NULL)) {
120
-        printf("Error mutex create: %s\n", strerror(-err));
121
-        exit(EXIT_FAILURE);
39
+    tasks.Init();
40
+    
41
+    /*if (tasks.AcceptClient()) {
42
+        tasks.Run();
43
+        
44
+        tasks.Join();
122 45
     }
46
+    
47
+    tasks.Stop();*/
48
+    
49
+    tasks.Run();
123 50
 
124
-    /* Creation du semaphore */
125
-    if (err = rt_sem_create(&sem_barrier, "truc", 0, S_FIFO)) {
126
-        printf("Error semaphore create 1: %d %s\n", err, strerror(-err));
127
-        exit(EXIT_FAILURE);
128
-    }
129
-    if (err = rt_sem_create(&sem_openComRobot, NULL, 0, S_FIFO)) {
130
-        printf("Error semaphore create 2: %s\n", strerror(-err));
131
-        exit(EXIT_FAILURE);
132
-    }
133
-    if (err = rt_sem_create(&sem_serverOk, NULL, 0, S_FIFO)) {
134
-        printf("Error semaphore create 3: %s\n", strerror(-err));
135
-        exit(EXIT_FAILURE);
136
-    }
137
-    if (err = rt_sem_create(&sem_startRobot, NULL, 0, S_FIFO)) {
138
-        printf("Error semaphore create 4: %s\n", strerror(-err));
139
-        exit(EXIT_FAILURE);
140
-    }
141
-
142
-    /* Creation des taches */
143
-    if (err = rt_task_create(&th_server, "th_server", 0, PRIORITY_TSERVER, 0)) {
144
-        printf("Error task create: %s\n", strerror(-err));
145
-        exit(EXIT_FAILURE);
146
-    }
147
-    if (err = rt_task_create(&th_receiveFromMon, "th_receiveFromMon", 0, PRIORITY_TRECEIVEFROMMON, 0)) {
148
-        printf("Error task create: %s\n", strerror(-err));
149
-        exit(EXIT_FAILURE);
150
-    }
151
-    if (err = rt_task_create(&th_sendToMon, "th_sendToMon", 0, PRIORITY_TSENDTOMON, 0)) {
152
-        printf("Error task create: %s\n", strerror(-err));
153
-        exit(EXIT_FAILURE);
154
-    }
155
-    if (err = rt_task_create(&th_openComRobot, "th_openComRobot", 0, PRIORITY_TOPENCOMROBOT, 0)) {
156
-        printf("Error task create: %s\n", strerror(-err));
157
-        exit(EXIT_FAILURE);
158
-    }
159
-    if (err = rt_task_create(&th_startRobot, "th_startRobot", 0, PRIORITY_TSTARTROBOT, 0)) {
160
-        printf("Error task create: %s\n", strerror(-err));
161
-        exit(EXIT_FAILURE);
162
-    }
163
-    if (err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0)) {
164
-        printf("Error task create: %s\n", strerror(-err));
165
-        exit(EXIT_FAILURE);
166
-    }
167
-
168
-    /* Creation des files de messages */
169
-    if (err = rt_queue_create(&q_messageToMon, "toto", MSG_QUEUE_SIZE * sizeof (MessageToRobot), MSG_QUEUE_SIZE, Q_FIFO)) {
170
-        printf("Error msg queue create: %s\n", strerror(-err));
171
-        exit(EXIT_FAILURE);
172
-    }
173
-}
174
-
175
-void startTasks() {
176
-
177
-    int err;
178
-
179
-    if (err = rt_task_start(&th_startRobot, &f_startRobot, NULL)) {
180
-        printf("Error task start: %s\n", strerror(-err));
181
-        exit(EXIT_FAILURE);
182
-    }
183
-
184
-    if (err = rt_task_start(&th_receiveFromMon, &f_receiveFromMon, NULL)) {
185
-        printf("Error task start: %s\n", strerror(-err));
186
-        exit(EXIT_FAILURE);
187
-    }
188
-    if (err = rt_task_start(&th_sendToMon, &f_sendToMon, NULL)) {
189
-        printf("Error task start: %s\n", strerror(-err));
190
-        exit(EXIT_FAILURE);
191
-    }
192
-    if (err = rt_task_start(&th_openComRobot, &f_openComRobot, NULL)) {
193
-        printf("Error task start: %s\n", strerror(-err));
194
-        exit(EXIT_FAILURE);
195
-    }
196
-    if (err = rt_task_start(&th_move, &f_move, NULL)) {
197
-        printf("Error task start: %s\n", strerror(-err));
198
-        exit(EXIT_FAILURE);
199
-    }
200
-    if (err = rt_task_start(&th_server, &f_server, NULL)) {
201
-        printf("Error task start: %s\n", strerror(-err));
202
-        exit(EXIT_FAILURE);
203
-    }
51
+    return 0;
204 52
 }
205 53
 
206
-void deleteTasks() {
207
-    rt_task_delete(&th_server);
208
-    rt_task_delete(&th_openComRobot);
209
-    rt_task_delete(&th_move);
210
-}

+ 12
- 0
software/raspberry/superviseur-robot/nbproject/Makefile-Debug.mk View File

@@ -42,8 +42,10 @@ OBJECTFILES= \
42 42
 	${OBJECTDIR}/lib/server.o \
43 43
 	${OBJECTDIR}/main.o \
44 44
 	${OBJECTDIR}/tasks.o \
45
+	${OBJECTDIR}/_ext/6cc0dc4a/camera.o \
45 46
 	${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \
46 47
 	${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \
48
+	${OBJECTDIR}/_ext/6cc0dc4a/img.o \
47 49
 	${OBJECTDIR}/tasks_pthread.o
48 50
 
49 51
 
@@ -106,6 +108,11 @@ ${OBJECTDIR}/tasks.o: tasks.cpp
106 108
 	${RM} "$@.d"
107 109
 	$(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
108 110
 
111
+${OBJECTDIR}/_ext/6cc0dc4a/camera.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp
112
+	${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
113
+	${RM} "$@.d"
114
+	$(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
115
+
109 116
 ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp
110 117
 	${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
111 118
 	${RM} "$@.d"
@@ -116,6 +123,11 @@ ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o: /home/dimercur/Documents/Travail/git/dumb
116 123
 	${RM} "$@.d"
117 124
 	$(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
118 125
 
126
+${OBJECTDIR}/_ext/6cc0dc4a/img.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp
127
+	${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
128
+	${RM} "$@.d"
129
+	$(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
130
+
119 131
 ${OBJECTDIR}/tasks_pthread.o: tasks_pthread.cpp
120 132
 	${MKDIR} -p ${OBJECTDIR}
121 133
 	${RM} "$@.d"

+ 14
- 26
software/raspberry/superviseur-robot/nbproject/Makefile-Debug__Pthread_.mk View File

@@ -35,14 +35,12 @@ OBJECTDIR=${CND_BUILDDIR}/${CND_CONF}/${CND_PLATFORM}
35 35
 
36 36
 # Object Files
37 37
 OBJECTFILES= \
38
-	${OBJECTDIR}/lib/message.o \
39 38
 	${OBJECTDIR}/lib/messages.o \
40
-	${OBJECTDIR}/lib/monitor.o \
41
-	${OBJECTDIR}/lib/robot.o \
42
-	${OBJECTDIR}/lib/server.o \
43 39
 	${OBJECTDIR}/main.o \
40
+	${OBJECTDIR}/_ext/6cc0dc4a/camera.o \
44 41
 	${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \
45 42
 	${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \
43
+	${OBJECTDIR}/_ext/6cc0dc4a/img.o \
46 44
 	${OBJECTDIR}/tasks_pthread.o
47 45
 
48 46
 
@@ -50,8 +48,8 @@ OBJECTFILES= \
50 48
 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
51 49
 
52 50
 # CC Compiler Flags
53
-CCFLAGS=-D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables
54
-CXXFLAGS=-D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables
51
+CCFLAGS=-D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables -Wno-pmf-conversions
52
+CXXFLAGS=-D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables -Wno-pmf-conversions
55 53
 
56 54
 # Fortran Compiler Flags
57 55
 FFLAGS=
@@ -70,36 +68,21 @@ ${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}/superviseur-robot: ${OBJECTFILES}
70 68
 	${MKDIR} -p ${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}
71 69
 	${LINK.cc} -o ${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}/superviseur-robot ${OBJECTFILES} ${LDLIBSOPTIONS} -lpthread -lrt
72 70
 
73
-${OBJECTDIR}/lib/message.o: lib/message.cpp
74
-	${MKDIR} -p ${OBJECTDIR}/lib
75
-	${RM} "$@.d"
76
-	$(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
77
-
78 71
 ${OBJECTDIR}/lib/messages.o: lib/messages.cpp
79 72
 	${MKDIR} -p ${OBJECTDIR}/lib
80 73
 	${RM} "$@.d"
81 74
 	$(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
82 75
 
83
-${OBJECTDIR}/lib/monitor.o: lib/monitor.cpp
84
-	${MKDIR} -p ${OBJECTDIR}/lib
85
-	${RM} "$@.d"
86
-	$(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
87
-
88
-${OBJECTDIR}/lib/robot.o: lib/robot.cpp
89
-	${MKDIR} -p ${OBJECTDIR}/lib
90
-	${RM} "$@.d"
91
-	$(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
92
-
93
-${OBJECTDIR}/lib/server.o: lib/server.cpp
94
-	${MKDIR} -p ${OBJECTDIR}/lib
95
-	${RM} "$@.d"
96
-	$(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
97
-
98 76
 ${OBJECTDIR}/main.o: main.cpp
99 77
 	${MKDIR} -p ${OBJECTDIR}
100 78
 	${RM} "$@.d"
101 79
 	$(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
102 80
 
81
+${OBJECTDIR}/_ext/6cc0dc4a/camera.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp
82
+	${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
83
+	${RM} "$@.d"
84
+	$(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
85
+
103 86
 ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp
104 87
 	${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
105 88
 	${RM} "$@.d"
@@ -110,6 +93,11 @@ ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o: /home/dimercur/Documents/Travail/git/dumb
110 93
 	${RM} "$@.d"
111 94
 	$(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
112 95
 
96
+${OBJECTDIR}/_ext/6cc0dc4a/img.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp
97
+	${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
98
+	${RM} "$@.d"
99
+	$(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
100
+
113 101
 ${OBJECTDIR}/tasks_pthread.o: tasks_pthread.cpp
114 102
 	${MKDIR} -p ${OBJECTDIR}
115 103
 	${RM} "$@.d"

+ 12
- 0
software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk View File

@@ -43,8 +43,10 @@ OBJECTFILES= \
43 43
 	${OBJECTDIR}/lib/server.o \
44 44
 	${OBJECTDIR}/main.o \
45 45
 	${OBJECTDIR}/tasks.o \
46
+	${OBJECTDIR}/_ext/6cc0dc4a/camera.o \
46 47
 	${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \
47 48
 	${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \
49
+	${OBJECTDIR}/_ext/6cc0dc4a/img.o \
48 50
 	${OBJECTDIR}/tasks_pthread.o
49 51
 
50 52
 
@@ -112,6 +114,11 @@ ${OBJECTDIR}/tasks.o: tasks.cpp
112 114
 	${RM} "$@.d"
113 115
 	$(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
114 116
 
117
+${OBJECTDIR}/_ext/6cc0dc4a/camera.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp
118
+	${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
119
+	${RM} "$@.d"
120
+	$(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
121
+
115 122
 ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp
116 123
 	${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
117 124
 	${RM} "$@.d"
@@ -122,6 +129,11 @@ ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o: /home/dimercur/Documents/Travail/git/dumb
122 129
 	${RM} "$@.d"
123 130
 	$(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
124 131
 
132
+${OBJECTDIR}/_ext/6cc0dc4a/img.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp
133
+	${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
134
+	${RM} "$@.d"
135
+	$(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
136
+
125 137
 ${OBJECTDIR}/tasks_pthread.o: tasks_pthread.cpp
126 138
 	${MKDIR} -p ${OBJECTDIR}
127 139
 	${RM} "$@.d"

+ 12
- 0
software/raspberry/superviseur-robot/nbproject/Makefile-Release.mk View File

@@ -43,8 +43,10 @@ OBJECTFILES= \
43 43
 	${OBJECTDIR}/lib/server.o \
44 44
 	${OBJECTDIR}/main.o \
45 45
 	${OBJECTDIR}/tasks.o \
46
+	${OBJECTDIR}/_ext/6cc0dc4a/camera.o \
46 47
 	${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o \
47 48
 	${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o \
49
+	${OBJECTDIR}/_ext/6cc0dc4a/img.o \
48 50
 	${OBJECTDIR}/tasks_pthread.o
49 51
 
50 52
 
@@ -112,6 +114,11 @@ ${OBJECTDIR}/tasks.o: tasks.cpp
112 114
 	${RM} "$@.d"
113 115
 	$(COMPILE.cc) -O2 -MMD -MP -MF "$@.d" -o ${OBJECTDIR}/tasks.o tasks.cpp
114 116
 
117
+${OBJECTDIR}/_ext/6cc0dc4a/camera.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp
118
+	${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
119
+	${RM} "$@.d"
120
+	$(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
121
+
115 122
 ${OBJECTDIR}/_ext/6cc0dc4a/commonitor.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp
116 123
 	${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
117 124
 	${RM} "$@.d"
@@ -122,6 +129,11 @@ ${OBJECTDIR}/_ext/6cc0dc4a/comrobot.o: /home/dimercur/Documents/Travail/git/dumb
122 129
 	${RM} "$@.d"
123 130
 	$(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
124 131
 
132
+${OBJECTDIR}/_ext/6cc0dc4a/img.o: /home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp
133
+	${MKDIR} -p ${OBJECTDIR}/_ext/6cc0dc4a
134
+	${RM} "$@.d"
135
+	$(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
136
+
125 137
 ${OBJECTDIR}/tasks_pthread.o: tasks_pthread.cpp
126 138
 	${MKDIR} -p ${OBJECTDIR}
127 139
 	${RM} "$@.d"

+ 47
- 23
software/raspberry/superviseur-robot/nbproject/configurations.xml View File

@@ -26,11 +26,11 @@
26 26
     <logicalFolder name="SourceFiles"
27 27
                    displayName="Source Files"
28 28
                    projectFiles="true">
29
-      <itemPath>./lib/camera.cpp</itemPath>
29
+      <itemPath>/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp</itemPath>
30 30
       <itemPath>/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp</itemPath>
31 31
       <itemPath>/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp</itemPath>
32 32
       <itemPath>./lib/image.cpp</itemPath>
33
-      <itemPath>./lib/img.cpp</itemPath>
33
+      <itemPath>/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp</itemPath>
34 34
       <itemPath>./main.cpp</itemPath>
35 35
       <itemPath>./lib/message.cpp</itemPath>
36 36
       <itemPath>./lib/messages.cpp</itemPath>
@@ -93,16 +93,12 @@
93 93
       </compileType>
94 94
       <item path="./gdbsudo.sh" ex="false" tool="3" flavor2="0">
95 95
       </item>
96
-      <item path="./lib/camera.cpp" ex="false" tool="3" flavor2="0">
97
-      </item>
98 96
       <item path="./lib/camera.h" ex="false" tool="3" flavor2="0">
99 97
       </item>
100 98
       <item path="./lib/definitions.h" ex="false" tool="3" flavor2="0">
101 99
       </item>
102 100
       <item path="./lib/image.h" ex="false" tool="3" flavor2="0">
103 101
       </item>
104
-      <item path="./lib/img.cpp" ex="false" tool="3" flavor2="0">
105
-      </item>
106 102
       <item path="./lib/img.h" ex="false" tool="3" flavor2="0">
107 103
       </item>
108 104
       <item path="./lib/message.cpp" ex="false" tool="1" flavor2="0">
@@ -131,6 +127,11 @@
131 127
       </item>
132 128
       <item path="./tasks.h" ex="false" tool="3" flavor2="0">
133 129
       </item>
130
+      <item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp"
131
+            ex="false"
132
+            tool="1"
133
+            flavor2="0">
134
+      </item>
134 135
       <item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp"
135 136
             ex="false"
136 137
             tool="1"
@@ -151,6 +152,11 @@
151 152
             tool="3"
152 153
             flavor2="0">
153 154
       </item>
155
+      <item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp"
156
+            ex="false"
157
+            tool="1"
158
+            flavor2="0">
159
+      </item>
154 160
       <item path="tasks_pthread.cpp" ex="false" tool="1" flavor2="0">
155 161
       </item>
156 162
       <item path="tasks_pthread.h" ex="false" tool="3" flavor2="0">
@@ -178,8 +184,6 @@
178 184
       </compileType>
179 185
       <item path="./gdbsudo.sh" ex="false" tool="3" flavor2="0">
180 186
       </item>
181
-      <item path="./lib/camera.cpp" ex="false" tool="3" flavor2="0">
182
-      </item>
183 187
       <item path="./lib/camera.h" ex="false" tool="3" flavor2="0">
184 188
       </item>
185 189
       <item path="./lib/definitions.h" ex="false" tool="3" flavor2="0">
@@ -188,8 +192,6 @@
188 192
       </item>
189 193
       <item path="./lib/image.h" ex="false" tool="3" flavor2="0">
190 194
       </item>
191
-      <item path="./lib/img.cpp" ex="false" tool="3" flavor2="0">
192
-      </item>
193 195
       <item path="./lib/img.h" ex="false" tool="3" flavor2="0">
194 196
       </item>
195 197
       <item path="./lib/message.cpp" ex="false" tool="1" flavor2="0">
@@ -218,6 +220,11 @@
218 220
       </item>
219 221
       <item path="./tasks.h" ex="false" tool="3" flavor2="0">
220 222
       </item>
223
+      <item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp"
224
+            ex="false"
225
+            tool="1"
226
+            flavor2="0">
227
+      </item>
221 228
       <item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp"
222 229
             ex="false"
223 230
             tool="1"
@@ -238,6 +245,11 @@
238 245
             tool="3"
239 246
             flavor2="0">
240 247
       </item>
248
+      <item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp"
249
+            ex="false"
250
+            tool="1"
251
+            flavor2="0">
252
+      </item>
241 253
       <item path="tasks_pthread.cpp" ex="false" tool="1" flavor2="0">
242 254
       </item>
243 255
       <item path="tasks_pthread.h" ex="false" tool="3" flavor2="0">
@@ -282,8 +294,6 @@
282 294
       </compileType>
283 295
       <item path="./gdbsudo.sh" ex="false" tool="3" flavor2="0">
284 296
       </item>
285
-      <item path="./lib/camera.cpp" ex="false" tool="3" flavor2="0">
286
-      </item>
287 297
       <item path="./lib/camera.h" ex="false" tool="3" flavor2="0">
288 298
       </item>
289 299
       <item path="./lib/definitions.h" ex="false" tool="3" flavor2="0">
@@ -292,8 +302,6 @@
292 302
       </item>
293 303
       <item path="./lib/image.h" ex="false" tool="3" flavor2="0">
294 304
       </item>
295
-      <item path="./lib/img.cpp" ex="false" tool="3" flavor2="0">
296
-      </item>
297 305
       <item path="./lib/img.h" ex="false" tool="3" flavor2="0">
298 306
       </item>
299 307
       <item path="./lib/message.cpp" ex="false" tool="1" flavor2="0">
@@ -322,6 +330,11 @@
322 330
       </item>
323 331
       <item path="./tasks.h" ex="false" tool="3" flavor2="0">
324 332
       </item>
333
+      <item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp"
334
+            ex="false"
335
+            tool="1"
336
+            flavor2="0">
337
+      </item>
325 338
       <item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp"
326 339
             ex="false"
327 340
             tool="1"
@@ -342,6 +355,11 @@
342 355
             tool="3"
343 356
             flavor2="0">
344 357
       </item>
358
+      <item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp"
359
+            ex="false"
360
+            tool="1"
361
+            flavor2="0">
362
+      </item>
345 363
       <item path="tasks_pthread.cpp" ex="false" tool="1" flavor2="0">
346 364
       </item>
347 365
       <item path="tasks_pthread.h" ex="false" tool="3" flavor2="0">
@@ -366,7 +384,7 @@
366 384
             <pElem>./</pElem>
367 385
             <pElem>./lib</pElem>
368 386
           </incDir>
369
-          <commandLine>-D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables</commandLine>
387
+          <commandLine>-D_GNU_SOURCE -D_REENTRANT -fasynchronous-unwind-tables -Wno-pmf-conversions</commandLine>
370 388
           <preprocessorList>
371 389
             <Elem>_WITH_TRACE_</Elem>
372 390
             <Elem>__FOR_PC__</Elem>
@@ -382,8 +400,6 @@
382 400
       </compileType>
383 401
       <item path="./gdbsudo.sh" ex="false" tool="3" flavor2="0">
384 402
       </item>
385
-      <item path="./lib/camera.cpp" ex="false" tool="3" flavor2="0">
386
-      </item>
387 403
       <item path="./lib/camera.h" ex="false" tool="3" flavor2="0">
388 404
       </item>
389 405
       <item path="./lib/definitions.h" ex="false" tool="3" flavor2="0">
@@ -392,11 +408,9 @@
392 408
       </item>
393 409
       <item path="./lib/image.h" ex="false" tool="3" flavor2="0">
394 410
       </item>
395
-      <item path="./lib/img.cpp" ex="false" tool="3" flavor2="0">
396
-      </item>
397 411
       <item path="./lib/img.h" ex="false" tool="3" flavor2="0">
398 412
       </item>
399
-      <item path="./lib/message.cpp" ex="false" tool="1" flavor2="9">
413
+      <item path="./lib/message.cpp" ex="true" tool="1" flavor2="9">
400 414
       </item>
401 415
       <item path="./lib/message.h" ex="false" tool="3" flavor2="0">
402 416
       </item>
@@ -404,15 +418,15 @@
404 418
       </item>
405 419
       <item path="./lib/messages.h" ex="false" tool="3" flavor2="0">
406 420
       </item>
407
-      <item path="./lib/monitor.cpp" ex="false" tool="1" flavor2="9">
421
+      <item path="./lib/monitor.cpp" ex="true" tool="1" flavor2="9">
408 422
       </item>
409 423
       <item path="./lib/monitor.h" ex="false" tool="3" flavor2="0">
410 424
       </item>
411
-      <item path="./lib/robot.cpp" ex="false" tool="1" flavor2="9">
425
+      <item path="./lib/robot.cpp" ex="true" tool="1" flavor2="9">
412 426
       </item>
413 427
       <item path="./lib/robot.h" ex="false" tool="3" flavor2="0">
414 428
       </item>
415
-      <item path="./lib/server.cpp" ex="false" tool="1" flavor2="9">
429
+      <item path="./lib/server.cpp" ex="true" tool="1" flavor2="9">
416 430
       </item>
417 431
       <item path="./lib/server.h" ex="false" tool="3" flavor2="0">
418 432
       </item>
@@ -420,6 +434,11 @@
420 434
       </item>
421 435
       <item path="./tasks.cpp" ex="true" tool="1" flavor2="9">
422 436
       </item>
437
+      <item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp"
438
+            ex="false"
439
+            tool="1"
440
+            flavor2="0">
441
+      </item>
423 442
       <item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp"
424 443
             ex="false"
425 444
             tool="1"
@@ -440,6 +459,11 @@
440 459
             tool="3"
441 460
             flavor2="0">
442 461
       </item>
462
+      <item path="/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp"
463
+            ex="false"
464
+            tool="1"
465
+            flavor2="0">
466
+      </item>
443 467
       <item path="tasks_pthread.cpp" ex="false" tool="1" flavor2="9">
444 468
       </item>
445 469
       <item path="tasks_pthread.h" ex="false" tool="3" flavor2="0">

+ 2
- 0
software/raspberry/superviseur-robot/nbproject/private/configurations.xml View File

@@ -116,6 +116,8 @@
116 116
         <gdb_interceptlist>
117 117
           <gdbinterceptoptions gdb_all="false" gdb_unhandled="true" gdb_unexpected="true"/>
118 118
         </gdb_interceptlist>
119
+        <gdb_signals>
120
+        </gdb_signals>
119 121
         <gdb_options>
120 122
           <DebugOptions>
121 123
           </DebugOptions>

+ 3
- 13
software/raspberry/superviseur-robot/nbproject/private/private.xml View File

@@ -7,25 +7,15 @@
7 7
     <editor-bookmarks xmlns="http://www.netbeans.org/ns/editor-bookmarks/2" lastBookmarkId="0"/>
8 8
     <open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2">
9 9
         <group>
10
-            <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/monitor.h</file>
11 10
             <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp</file>
12 11
             <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/comrobot.h</file>
12
+            <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks_pthread.h</file>
13 13
             <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.h</file>
14
-            <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.h</file>
15
-            <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/img.cpp</file>
14
+            <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/main.cpp</file>
16 15
             <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/messages.h</file>
17
-            <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/robot.h</file>
18 16
             <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp</file>
19
-            <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.cpp</file>
20
-            <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks_pthread.cpp</file>
21
-            <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/image.h</file>
22
-            <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/server.h</file>
23
-            <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/camera.h</file>
24
-            <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks.cpp</file>
25
-            <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks_pthread.h</file>
26
-            <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/message.h</file>
27
-            <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/main.cpp</file>
28 17
             <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/lib/messages.cpp</file>
18
+            <file>file:/home/dimercur/Documents/Travail/git/dumber/software/raspberry/superviseur-robot/tasks_pthread.cpp</file>
29 19
         </group>
30 20
     </open-files>
31 21
 </project-private>

+ 13
- 9
software/raspberry/superviseur-robot/tasks.cpp View File

@@ -15,16 +15,18 @@
15 15
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
16 16
  */
17 17
 
18
-/**
19
- * \file      functions.h
20
- * \author    PE.Hladik
21
- * \version   1.0
22
- * \date      06/06/2017
23
- * \brief     Miscellaneous functions used for destijl project.
24
- */
25
-
26 18
 #include "tasks.h"
27 19
 
20
+#ifndef __WITH_PTHREAD__
21
+
22
+// Déclaration des priorités des taches
23
+#define PRIORITY_TSERVER 30
24
+#define PRIORITY_TOPENCOMROBOT 20
25
+#define PRIORITY_TMOVE 10
26
+#define PRIORITY_TSENDTOMON 25
27
+#define PRIORITY_TRECEIVEFROMMON 22
28
+#define PRIORITY_TSTARTROBOT 20
29
+
28 30
 char mode_start;
29 31
 
30 32
 void write_in_queue(RT_QUEUE *, MessageToMon);
@@ -258,4 +260,6 @@ void write_in_queue(RT_QUEUE *queue, MessageToMon msg) {
258 260
     buff = rt_queue_alloc(&q_messageToMon, sizeof (MessageToMon));
259 261
     memcpy(buff, &msg, sizeof (MessageToMon));
260 262
     rt_queue_send(&q_messageToMon, buff, sizeof (MessageToMon), Q_NORMAL);
261
-}
263
+}
264
+
265
+#endif // __WITH_PTHREAD__

+ 104
- 75
software/raspberry/superviseur-robot/tasks.h View File

@@ -15,17 +15,10 @@
15 15
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
16 16
  */
17 17
 
18
-/**
19
- * \file      functions.h
20
- * \author    PE.Hladik
21
- * \version   1.0
22
- * \date      06/06/2017
23
- * \brief     Miscellaneous functions used for destijl project.
24
- */
25
-
26 18
 #ifndef __TASKS_H__
27 19
 #define __TASKS_H__
28 20
 
21
+#ifndef __WITH_PTHREAD__
29 22
 #include <stdio.h>
30 23
 #include <stdlib.h>
31 24
 #include <unistd.h>
@@ -37,71 +30,107 @@
37 30
 #include <alchemy/sem.h>
38 31
 #include <alchemy/queue.h>
39 32
 
40
-#include "monitor.h"
41
-#include "robot.h"
42
-#include "image.h"
43
-#include "message.h"
44
-#include "server.h"
45
-
46
-extern RT_TASK th_server;
47
-extern RT_TASK th_sendToMon;
48
-extern RT_TASK th_receiveFromMon;
49
-extern RT_TASK th_openComRobot;
50
-extern RT_TASK th_startRobot;
51
-extern RT_TASK th_move;
52
-
53
-extern RT_MUTEX mutex_robotStarted;
54
-extern RT_MUTEX mutex_move;
55
-
56
-extern RT_SEM sem_barrier;
57
-extern RT_SEM sem_openComRobot;
58
-extern RT_SEM sem_serverOk;
59
-extern RT_SEM sem_startRobot;
60
-
61
-extern RT_QUEUE q_messageToMon;
62
-
63
-extern int etatCommMoniteur;
64
-extern int robotStarted;
65
-extern char robotMove;
66
-
67
-extern int MSG_QUEUE_SIZE;
68
-
69
-extern int PRIORITY_TSERVER;
70
-extern int PRIORITY_TOPENCOMROBOT;
71
-extern int PRIORITY_TMOVE;
72
-extern int PRIORITY_TSENDTOMON;
73
-extern int PRIORITY_TRECEIVEFROMMON;
74
-extern int PRIORITY_TSTARTROBOT;
75
-
76
-/**
77
- * \brief       Thread handling server communication.
78
- */ 
79
-void f_server(void *arg);
80
-
81
-/**
82
- * \brief       Thread handling communication to monitor.
83
- */ 
84
-void f_sendToMon(void *arg);
85
-
86
-/**
87
- * \brief       Thread handling communication from monitor.
88
- */ 
89
-void f_receiveFromMon(void *arg);
90
-
91
-/**
92
- * \brief       Thread handling opening of robot communication.
93
- */ 
94
-void f_openComRobot(void * arg);
95
-
96
-/**
97
- * \brief       Thread handling robot mouvements.
98
- */ 
99
-void f_move(void *arg);
100
-
101
-/**
102
- * \brief       Thread handling robot activation.
103
- */ 
104
-void f_startRobot(void *arg);
105
-
106
-#endif /* FUNCTIONS_H */
33
+//#include "monitor.h"
34
+//#include "robot.h"
35
+//#include "image.h"
36
+//#include "message.h"
37
+//#include "server.h"
38
+
39
+#include "messages.h"
40
+#include "commonitor.h"
41
+#include "comrobot.h"
42
+
43
+using namespace std;
44
+
45
+class Tasks {
46
+public:
47
+    /**
48
+     * @brief Initialisation des structures de l'application (tâches, mutex, 
49
+     * semaphore, etc.)
50
+     */
51
+    void Init();
52
+
53
+    /**
54
+     * @brief Démarrage des tâches
55
+     */
56
+    void Run();
57
+
58
+    /**
59
+     * @brief Arrêt des tâches
60
+     */
61
+    void Stop();
62
+    
63
+    /**
64
+     */
65
+    void Join() {
66
+        rt_sem_broadcast(&sem_barrier);
67
+        pause();
68
+    }
69
+    
70
+    /**
71
+     */
72
+    bool AcceptClient() {
73
+        return false;
74
+    }
75
+    
76
+private:
77
+    ComMonitor monitor;
78
+    ComRobot robot;
79
+    
80
+    RT_TASK th_server;
81
+    RT_TASK th_sendToMon;
82
+    RT_TASK th_receiveFromMon;
83
+    RT_TASK th_openComRobot;
84
+    RT_TASK th_startRobot;
85
+    RT_TASK th_move;
86
+
87
+    RT_MUTEX mutex_robotStarted;
88
+    RT_MUTEX mutex_move;
89
+
90
+    RT_SEM sem_barrier;
91
+    RT_SEM sem_openComRobot;
92
+    RT_SEM sem_serverOk;
93
+    RT_SEM sem_startRobot;
94
+
95
+    RT_QUEUE q_messageToMon;
96
+
97
+    int etatCommMoniteur;
98
+    int robotStarted;
99
+    char robotMove;
100
+
101
+    int MSG_QUEUE_SIZE;
102
+
103
+    /**
104
+     * \brief       Thread handling server communication.
105
+     */
106
+    void f_server(void *arg);
107
+
108
+    /**
109
+     * \brief       Thread handling communication to monitor.
110
+     */
111
+    void f_sendToMon(void *arg);
112
+
113
+    /**
114
+     * \brief       Thread handling communication from monitor.
115
+     */
116
+    void f_receiveFromMon(void *arg);
117
+
118
+    /**
119
+     * \brief       Thread handling opening of robot communication.
120
+     */
121
+    void f_openComRobot(void * arg);
122
+
123
+    /**
124
+     * \brief       Thread handling robot mouvements.
125
+     */
126
+    void f_move(void *arg);
127
+
128
+    /**
129
+     * \brief       Thread handling robot activation.
130
+     */
131
+    void f_startRobot(void *arg);
132
+};
133
+
134
+#endif // __WITH_PTHREAD__
135
+#endif // __TASKS_H__ 
107 136
 

+ 297
- 209
software/raspberry/superviseur-robot/tasks_pthread.cpp View File

@@ -15,249 +15,337 @@
15 15
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
16 16
  */
17 17
 
18
-/**
19
- * \file      functions.h
20
- * \author    PE.Hladik
21
- * \version   1.0
22
- * \date      06/06/2017
23
- * \brief     Miscellaneous functions used for destijl project.
24
- */
25
-
26 18
 #include "tasks_pthread.h"
19
+#include <time.h>
27 20
 
28 21
 #ifdef __WITH_PTHREAD__
29
-char mode_start;
30 22
 
31
-void write_in_queue(RT_QUEUE *, MessageToMon);
23
+// Déclaration des priorités des taches
24
+#define PRIORITY_TSERVER 30
25
+#define PRIORITY_TOPENCOMROBOT 20
26
+#define PRIORITY_TMOVE 10
27
+#define PRIORITY_TSENDTOMON 25
28
+#define PRIORITY_TRECEIVEFROMMON 22
29
+#define PRIORITY_TSTARTROBOT 20
32 30
 
33
-void f_server(void *arg) {
34
-    int err;
35
-    /* INIT */
36
-    RT_TASK_INFO info;
37
-    rt_task_inquire(NULL, &info);
38
-    printf("Init %s\n", info.name);
39
-    rt_sem_p(&sem_barrier, TM_INFINITE);
31
+/*
32
+ * Some remarks:
33
+ * 1- This program is mostly a template. It shows you how to create tasks, semaphore
34
+ *   message queues, mutex ... and how to use them
35
+ * 
36
+ * 2- semDumber is, as name say, useless. Its goal is only to show you how to use semaphore
37
+ * 
38
+ * 3- Data flow is probably not optimal
39
+ * 
40
+ * 4- Take into account that ComRobot::Write will block your task when serial buffer is full,
41
+ *   time for internal buffer to flush
42
+ * 
43
+ * 5- Same behavior existe for ComMonitor::Write !
44
+ * 
45
+ * 6- When you want to write something in terminal, use cout and terminate with endl and flush
46
+ * 
47
+ * 7- Good luck !
48
+ */
40 49
 
41
-    err=openServer(DEFAULT_SERVER_PORT);
50
+void Tasks::Init() {
51
+    int status; 
52
+    
53
+    /* Open com port with STM32 */
54
+    cout << "Open serial com (";
55
+    status = robot.Open("/dev/ttyUSB1");
56
+    cout << status;
57
+    cout << ")" << endl;
42 58
 
43
-    if (err < 0) {
44
-        printf("Failed to start server: %s\n", strerror(-err));
45
-        exit(EXIT_FAILURE);
46
-    } else {
47
-#ifdef _WITH_TRACE_
48
-        printf("%s: server started\n", info.name);
49
-#endif
50
-        //Waiting for a client to connect
51
-        err=acceptClient();
52
-        
53
-        if (err<0) {
54
-            printf("Client accept failed: %s\n", strerror(-err));
55
-            exit(EXIT_FAILURE);
56
-        }
59
+    if (status >= 0) {
60
+        // Open server
57 61
 
58
-#ifdef _WITH_TRACE_
59
-        printf ("client connected: %d\n", err);
60
-        printf ("Rock'n'roll baby !\n");
61
-#endif        
62
-        rt_sem_broadcast(&sem_serverOk);
63
-    }
64
-}
62
+        status = monitor.Open(SERVER_PORT);
63
+        cout << "Open server on port " << SERVER_PORT << " (" << status << ")" << endl;
65 64
 
66
-void f_sendToMon(void * arg) {
67
-    int err;
68
-    MessageToMon msg;
65
+        if (status < 0) throw std::runtime_error {
66
+            "Unable to start server on port " + std::to_string(SERVER_PORT)
67
+        };
68
+    } else
69
+        throw std::runtime_error {
70
+        "Unable to open serial port /dev/ttyS0 "
71
+    };
72
+}
69 73
 
70
-    /* INIT */
71
-    RT_TASK_INFO info;
72
-    rt_task_inquire(NULL, &info);
73
-    printf("Init %s\n", info.name);
74
-    rt_sem_p(&sem_barrier, TM_INFINITE);
74
+void Tasks::Run() {
75
+    Message *msgRcv;
76
+    Message *msgSend;
77
+    int counter = 3;
75 78
 
76
-#ifdef _WITH_TRACE_
77
-    printf("%s : waiting for sem_serverOk\n", info.name);
78
-#endif
79
-    rt_sem_p(&sem_serverOk, TM_INFINITE);
80
-    while (1) {
79
+    //    threadServer=new thread((void (*)(void*)) &Tasks::ServerTask,this);
80
+    //    threadSendToMon=new thread((void (*)(void*)) &Tasks::SendToMonTask,this);
81
+    //    threadTimer=new thread((void (*)(void*)) &Tasks::TimerTask,this);
81 82
 
82
-#ifdef _WITH_TRACE_
83
-        printf("%s : waiting for a message in queue\n", info.name);
84
-#endif
85
-        if (rt_queue_read(&q_messageToMon, &msg, sizeof (MessageToRobot), TM_INFINITE) >= 0) {
86
-#ifdef _WITH_TRACE_
87
-            printf("%s : message {%s,%s} in queue\n", info.name, msg.header, (char*)msg.data);
88
-#endif
83
+    msgSend = ComRobot::Ping();
84
+    cout << "Send => " << msgSend->ToString() << endl << flush;
85
+    msgRcv = robot.SendCommand(msgSend, MESSAGE_ANSWER_ACK, 3);
86
+    cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
89 87
 
90
-            send_message_to_monitor(msg.header, msg.data);
91
-            free_msgToMon_data(&msg);
92
-            rt_queue_free(&q_messageToMon, &msg);
93
-        } else {
94
-            printf("Error msg queue write: %s\n", strerror(-err));
95
-        }
96
-    }
97
-}
88
+    delete(msgRcv);
98 89
 
99
-void f_receiveFromMon(void *arg) {
100
-    MessageFromMon msg;
101
-    int err;
90
+    msgSend = ComRobot::StartWithoutWD();
91
+    cout << "Send => " << msgSend->ToString() << endl << flush;
92
+    msgRcv = robot.SendCommand(msgSend, MESSAGE_ANSWER_ACK, 3);
93
+    cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
102 94
 
103
-    /* INIT */
104
-    RT_TASK_INFO info;
105
-    rt_task_inquire(NULL, &info);
106
-    printf("Init %s\n", info.name);
107
-    rt_sem_p(&sem_barrier, TM_INFINITE);
95
+    delete(msgRcv);
108 96
 
109
-#ifdef _WITH_TRACE_
110
-    printf("%s : waiting for sem_serverOk\n", info.name);
111
-#endif
112
-    rt_sem_p(&sem_serverOk, TM_INFINITE);
113
-    do {
114
-#ifdef _WITH_TRACE_
115
-        printf("%s : waiting for a message from monitor\n", info.name);
116
-#endif
117
-        err = receive_message_from_monitor(msg.header, msg.data);
118
-#ifdef _WITH_TRACE_
119
-        printf("%s: msg {header:%s,data=%s} received from UI\n", info.name, msg.header, msg.data);
120
-#endif
121
-        if (strcmp(msg.header, HEADER_MTS_COM_DMB) == 0) {
122
-            if (msg.data[0] == OPEN_COM_DMB) { // Open communication supervisor-robot
123
-#ifdef _WITH_TRACE_
124
-                printf("%s: message open Xbee communication\n", info.name);
125
-#endif
126
-                rt_sem_v(&sem_openComRobot);
127
-            }
128
-        } else if (strcmp(msg.header, HEADER_MTS_DMB_ORDER) == 0) {
129
-            if (msg.data[0] == DMB_START_WITHOUT_WD) { // Start robot
130
-#ifdef _WITH_TRACE_
131
-                printf("%s: message start robot\n", info.name);
132
-#endif 
133
-                rt_sem_v(&sem_startRobot);
97
+    msgSend = ComRobot::Move(1000);
98
+    cout << "Send => " << msgSend->ToString() << endl << flush;
99
+    msgRcv = robot.SendCommand(msgSend, MESSAGE_ANSWER_ACK, 3);
100
+    cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
134 101
 
135
-            } else if ((msg.data[0] == DMB_GO_BACK)
136
-                    || (msg.data[0] == DMB_GO_FORWARD)
137
-                    || (msg.data[0] == DMB_GO_LEFT)
138
-                    || (msg.data[0] == DMB_GO_RIGHT)
139
-                    || (msg.data[0] == DMB_STOP_MOVE)) {
102
+    delete(msgRcv);
140 103
 
141
-                rt_mutex_acquire(&mutex_move, TM_INFINITE);
142
-                robotMove = msg.data[0];
143
-                rt_mutex_release(&mutex_move);
144
-#ifdef _WITH_TRACE_
145
-                printf("%s: message update movement with %c\n", info.name, robotMove);
146
-#endif
104
+    msgSend = ComRobot::GetBattery();
105
+    cout << "Send => " << msgSend->ToString() << endl << flush;
106
+    msgRcv = robot.SendCommand(msgSend, MESSAGE_ROBOT_BATTERY_LEVEL, 3);
107
+    cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
147 108
 
148
-            }
149
-        }
150
-    } while (err > 0);
109
+    delete(msgRcv);
110
+}
151 111
 
112
+void Tasks::Stop() {
113
+    monitor.Close();
114
+    robot.Close();
152 115
 }
153 116
 
154
-void f_openComRobot(void * arg) {
117
+void Tasks::ServerTask(void *arg) {
155 118
     int err;
156
-
157
-    /* INIT */
158
-    RT_TASK_INFO info;
159
-    rt_task_inquire(NULL, &info);
160
-    printf("Init %s\n", info.name);
161
-    rt_sem_p(&sem_barrier, TM_INFINITE);
162
-
119
+    cout << "Start " << __PRETTY_FUNCTION__ <<endl<<flush;
120
+    
163 121
     while (1) {
164
-#ifdef _WITH_TRACE_
165
-        printf("%s : Wait sem_openComRobot\n", info.name);
166
-#endif
167
-        rt_sem_p(&sem_openComRobot, TM_INFINITE);
168
-#ifdef _WITH_TRACE_
169
-        printf("%s : sem_openComRobot arrived => open communication robot\n", info.name);
170
-#endif
171
-        err = open_communication_robot();
172
-        if (err == 0) {
173
-#ifdef _WITH_TRACE_
174
-            printf("%s : the communication is opened\n", info.name);
175
-#endif
176
-            MessageToMon msg;
177
-            set_msgToMon_header(&msg, (char*)HEADER_STM_ACK);
178
-            write_in_queue(&q_messageToMon, msg);
179
-        } else {
180
-            MessageToMon msg;
181
-            set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK);
182
-            write_in_queue(&q_messageToMon, msg);
183
-        }
122
+        
184 123
     }
185 124
 }
186 125
 
187
-void f_startRobot(void * arg) {
188
-    int err;
189
-
190
-    /* INIT */
191
-    RT_TASK_INFO info;
192
-    rt_task_inquire(NULL, &info);
193
-    printf("Init %s\n", info.name);
194
-    rt_sem_p(&sem_barrier, TM_INFINITE);
195
-
126
+void Tasks::TimerTask(void* arg) {
127
+    struct timespec tim, tim2;
128
+    tim.tv_sec = 0;
129
+    tim.tv_nsec = 100000000;
130
+    
131
+    cout << "Start " << __PRETTY_FUNCTION__ <<endl<<flush;
196 132
     while (1) {
197
-#ifdef _WITH_TRACE_
198
-        printf("%s : Wait sem_startRobot\n", info.name);
199
-#endif
200
-        rt_sem_p(&sem_startRobot, TM_INFINITE);
201
-#ifdef _WITH_TRACE_
202
-        printf("%s : sem_startRobot arrived => Start robot\n", info.name);
203
-#endif
204
-        err = send_command_to_robot(DMB_START_WITHOUT_WD);
205
-        if (err == 0) {
206
-#ifdef _WITH_TRACE_
207
-            printf("%s : the robot is started\n", info.name);
208
-#endif
209
-            rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
210
-            robotStarted = 1;
211
-            rt_mutex_release(&mutex_robotStarted);
212
-            MessageToMon msg;
213
-            set_msgToMon_header(&msg, (char*)HEADER_STM_ACK);
214
-            write_in_queue(&q_messageToMon, msg);
215
-        } else {
216
-            MessageToMon msg;
217
-            set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK);
218
-            write_in_queue(&q_messageToMon, msg);
133
+        //std::this_thread::sleep_for(std::chrono::seconds )
134
+        //sleep(1);
135
+        if (nanosleep(&tim, &tim2) < 0) {
136
+            printf("Nano sleep system call failed \n");
137
+            return;
219 138
         }
139
+        
140
+        mutexTimer.unlock();
220 141
     }
221 142
 }
222 143
 
223
-void f_move(void *arg) {
224
-    /* INIT */
225
-    RT_TASK_INFO info;
226
-    rt_task_inquire(NULL, &info);
227
-    printf("Init %s\n", info.name);
228
-    rt_sem_p(&sem_barrier, TM_INFINITE);
229
-
230
-    /* PERIODIC START */
231
-#ifdef _WITH_PERIODIC_TRACE_
232
-    printf("%s: start period\n", info.name);
233
-#endif
234
-    rt_task_set_periodic(NULL, TM_NOW, 100000000);
144
+void Tasks::SendToMonTask(void* arg) {
145
+    
146
+    cout << "Start " << __PRETTY_FUNCTION__ <<endl<<flush;
147
+    
235 148
     while (1) {
236
-#ifdef _WITH_PERIODIC_TRACE_
237
-        printf("%s: Wait period \n", info.name);
238
-#endif
239
-        rt_task_wait_period(NULL);
240
-#ifdef _WITH_PERIODIC_TRACE_
241
-        printf("%s: Periodic activation\n", info.name);
242
-        printf("%s: move equals %c\n", info.name, robotMove);
243
-#endif
244
-        rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
245
-        if (robotStarted) {
246
-            rt_mutex_acquire(&mutex_move, TM_INFINITE);
247
-            send_command_to_robot(robotMove);
248
-            rt_mutex_release(&mutex_move);
249
-#ifdef _WITH_TRACE_
250
-            printf("%s: the movement %c was sent\n", info.name, robotMove);
251
-#endif            
252
-        }
253
-        rt_mutex_release(&mutex_robotStarted);
149
+        
254 150
     }
255 151
 }
256 152
 
257
-void write_in_queue(RT_QUEUE *queue, MessageToMon msg) {
258
-    void *buff;
259
-    buff = rt_queue_alloc(&q_messageToMon, sizeof (MessageToMon));
260
-    memcpy(buff, &msg, sizeof (MessageToMon));
261
-    rt_queue_send(&q_messageToMon, buff, sizeof (MessageToMon), Q_NORMAL);
262
-}
153
+//void Tasks::f_sendToMon(void * arg) {
154
+//    int err;
155
+//    MessageToMon msg;
156
+//
157
+//    /* INIT */
158
+//    RT_TASK_INFO info;
159
+//    rt_task_inquire(NULL, &info);
160
+//    printf("Init %s\n", info.name);
161
+//    rt_sem_p(&sem_barrier, TM_INFINITE);
162
+//
163
+//#ifdef _WITH_TRACE_
164
+//    printf("%s : waiting for sem_serverOk\n", info.name);
165
+//#endif
166
+//    rt_sem_p(&sem_serverOk, TM_INFINITE);
167
+//    while (1) {
168
+//
169
+//#ifdef _WITH_TRACE_
170
+//        printf("%s : waiting for a message in queue\n", info.name);
171
+//#endif
172
+//        if (rt_queue_read(&q_messageToMon, &msg, sizeof (MessageToRobot), TM_INFINITE) >= 0) {
173
+//#ifdef _WITH_TRACE_
174
+//            printf("%s : message {%s,%s} in queue\n", info.name, msg.header, (char*)msg.data);
175
+//#endif
176
+//
177
+//            send_message_to_monitor(msg.header, msg.data);
178
+//            free_msgToMon_data(&msg);
179
+//            rt_queue_free(&q_messageToMon, &msg);
180
+//        } else {
181
+//            printf("Error msg queue write: %s\n", strerror(-err));
182
+//        }
183
+//    }
184
+//}
185
+//
186
+//void Tasks::f_receiveFromMon(void *arg) {
187
+//    MessageFromMon msg;
188
+//    int err;
189
+//
190
+//    /* INIT */
191
+//    RT_TASK_INFO info;
192
+//    rt_task_inquire(NULL, &info);
193
+//    printf("Init %s\n", info.name);
194
+//    rt_sem_p(&sem_barrier, TM_INFINITE);
195
+//
196
+//#ifdef _WITH_TRACE_
197
+//    printf("%s : waiting for sem_serverOk\n", info.name);
198
+//#endif
199
+//    rt_sem_p(&sem_serverOk, TM_INFINITE);
200
+//    do {
201
+//#ifdef _WITH_TRACE_
202
+//        printf("%s : waiting for a message from monitor\n", info.name);
203
+//#endif
204
+//        err = receive_message_from_monitor(msg.header, msg.data);
205
+//#ifdef _WITH_TRACE_
206
+//        printf("%s: msg {header:%s,data=%s} received from UI\n", info.name, msg.header, msg.data);
207
+//#endif
208
+//        if (strcmp(msg.header, HEADER_MTS_COM_DMB) == 0) {
209
+//            if (msg.data[0] == OPEN_COM_DMB) { // Open communication supervisor-robot
210
+//#ifdef _WITH_TRACE_
211
+//                printf("%s: message open Xbee communication\n", info.name);
212
+//#endif
213
+//                rt_sem_v(&sem_openComRobot);
214
+//            }
215
+//        } else if (strcmp(msg.header, HEADER_MTS_DMB_ORDER) == 0) {
216
+//            if (msg.data[0] == DMB_START_WITHOUT_WD) { // Start robot
217
+//#ifdef _WITH_TRACE_
218
+//                printf("%s: message start robot\n", info.name);
219
+//#endif 
220
+//                rt_sem_v(&sem_startRobot);
221
+//
222
+//            } else if ((msg.data[0] == DMB_GO_BACK)
223
+//                    || (msg.data[0] == DMB_GO_FORWARD)
224
+//                    || (msg.data[0] == DMB_GO_LEFT)
225
+//                    || (msg.data[0] == DMB_GO_RIGHT)
226
+//                    || (msg.data[0] == DMB_STOP_MOVE)) {
227
+//
228
+//                rt_mutex_acquire(&mutex_move, TM_INFINITE);
229
+//                robotMove = msg.data[0];
230
+//                rt_mutex_release(&mutex_move);
231
+//#ifdef _WITH_TRACE_
232
+//                printf("%s: message update movement with %c\n", info.name, robotMove);
233
+//#endif
234
+//
235
+//            }
236
+//        }
237
+//    } while (err > 0);
238
+//
239
+//}
240
+//
241
+//void Tasks::f_openComRobot(void * arg) {
242
+//    int err;
243
+//
244
+//    /* INIT */
245
+//    RT_TASK_INFO info;
246
+//    rt_task_inquire(NULL, &info);
247
+//    printf("Init %s\n", info.name);
248
+//    rt_sem_p(&sem_barrier, TM_INFINITE);
249
+//
250
+//    while (1) {
251
+//#ifdef _WITH_TRACE_
252
+//        printf("%s : Wait sem_openComRobot\n", info.name);
253
+//#endif
254
+//        rt_sem_p(&sem_openComRobot, TM_INFINITE);
255
+//#ifdef _WITH_TRACE_
256
+//        printf("%s : sem_openComRobot arrived => open communication robot\n", info.name);
257
+//#endif
258
+//        err = open_communication_robot();
259
+//        if (err == 0) {
260
+//#ifdef _WITH_TRACE_
261
+//            printf("%s : the communication is opened\n", info.name);
262
+//#endif
263
+//            MessageToMon msg;
264
+//            set_msgToMon_header(&msg, (char*)HEADER_STM_ACK);
265
+//            write_in_queue(&q_messageToMon, msg);
266
+//        } else {
267
+//            MessageToMon msg;
268
+//            set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK);
269
+//            write_in_queue(&q_messageToMon, msg);
270
+//        }
271
+//    }
272
+//}
273
+//
274
+//void Tasks::f_startRobot(void * arg) {
275
+//    int err;
276
+//
277
+//    /* INIT */
278
+//    RT_TASK_INFO info;
279
+//    rt_task_inquire(NULL, &info);
280
+//    printf("Init %s\n", info.name);
281
+//    rt_sem_p(&sem_barrier, TM_INFINITE);
282
+//
283
+//    while (1) {
284
+//#ifdef _WITH_TRACE_
285
+//        printf("%s : Wait sem_startRobot\n", info.name);
286
+//#endif
287
+//        rt_sem_p(&sem_startRobot, TM_INFINITE);
288
+//#ifdef _WITH_TRACE_
289
+//        printf("%s : sem_startRobot arrived => Start robot\n", info.name);
290
+//#endif
291
+//        err = send_command_to_robot(DMB_START_WITHOUT_WD);
292
+//        if (err == 0) {
293
+//#ifdef _WITH_TRACE_
294
+//            printf("%s : the robot is started\n", info.name);
295
+//#endif
296
+//            rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
297
+//            robotStarted = 1;
298
+//            rt_mutex_release(&mutex_robotStarted);
299
+//            MessageToMon msg;
300
+//            set_msgToMon_header(&msg, (char*)HEADER_STM_ACK);
301
+//            write_in_queue(&q_messageToMon, msg);
302
+//        } else {
303
+//            MessageToMon msg;
304
+//            set_msgToMon_header(&msg, (char*)HEADER_STM_NO_ACK);
305
+//            write_in_queue(&q_messageToMon, msg);
306
+//        }
307
+//    }
308
+//}
309
+//
310
+//void Tasks::f_move(void *arg) {
311
+//    /* INIT */
312
+//    RT_TASK_INFO info;
313
+//    rt_task_inquire(NULL, &info);
314
+//    printf("Init %s\n", info.name);
315
+//    rt_sem_p(&sem_barrier, TM_INFINITE);
316
+//
317
+//    /* PERIODIC START */
318
+//#ifdef _WITH_PERIODIC_TRACE_
319
+//    printf("%s: start period\n", info.name);
320
+//#endif
321
+//    rt_task_set_periodic(NULL, TM_NOW, 100000000);
322
+//    while (1) {
323
+//#ifdef _WITH_PERIODIC_TRACE_
324
+//        printf("%s: Wait period \n", info.name);
325
+//#endif
326
+//        rt_task_wait_period(NULL);
327
+//#ifdef _WITH_PERIODIC_TRACE_
328
+//        printf("%s: Periodic activation\n", info.name);
329
+//        printf("%s: move equals %c\n", info.name, robotMove);
330
+//#endif
331
+//        rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
332
+//        if (robotStarted) {
333
+//            rt_mutex_acquire(&mutex_move, TM_INFINITE);
334
+//            send_command_to_robot(robotMove);
335
+//            rt_mutex_release(&mutex_move);
336
+//#ifdef _WITH_TRACE_
337
+//            printf("%s: the movement %c was sent\n", info.name, robotMove);
338
+//#endif            
339
+//        }
340
+//        rt_mutex_release(&mutex_robotStarted);
341
+//    }
342
+//}
343
+//
344
+//void write_in_queue(RT_QUEUE *queue, MessageToMon msg) {
345
+//    void *buff;
346
+//    buff = rt_queue_alloc(&q_messageToMon, sizeof (MessageToMon));
347
+//    memcpy(buff, &msg, sizeof (MessageToMon));
348
+//    rt_queue_send(&q_messageToMon, buff, sizeof (MessageToMon), Q_NORMAL);
349
+//}
350
+
263 351
 #endif //__WITH_PTHREAD__

+ 101
- 70
software/raspberry/superviseur-robot/tasks_pthread.h View File

@@ -15,14 +15,6 @@
15 15
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
16 16
  */
17 17
 
18
-/**
19
- * \file      functions.h
20
- * \author    PE.Hladik
21
- * \version   1.0
22
- * \date      06/06/2017
23
- * \brief     Miscellaneous functions used for destijl project.
24
- */
25
-
26 18
 #ifndef __TASKS_H__
27 19
 #define __TASKS_H__
28 20
 
@@ -31,73 +23,112 @@
31 23
 #include <stdlib.h>
32 24
 #include <unistd.h>
33 25
 
34
-#include <sys/mman.h>
35
-
36
-#include "monitor.h"
37
-#include "robot.h"
38
-#include "image.h"
39
-#include "message.h"
40
-#include "server.h"
41
-
42
-extern RT_TASK th_server;
43
-extern RT_TASK th_sendToMon;
44
-extern RT_TASK th_receiveFromMon;
45
-extern RT_TASK th_openComRobot;
46
-extern RT_TASK th_startRobot;
47
-extern RT_TASK th_move;
48
-
49
-extern RT_MUTEX mutex_robotStarted;
50
-extern RT_MUTEX mutex_move;
51
-
52
-extern RT_SEM sem_barrier;
53
-extern RT_SEM sem_openComRobot;
54
-extern RT_SEM sem_serverOk;
55
-extern RT_SEM sem_startRobot;
56
-
57
-extern RT_QUEUE q_messageToMon;
58
-
59
-extern int etatCommMoniteur;
60
-extern int robotStarted;
61
-extern char robotMove;
26
+//#include "monitor.h"
27
+//#include "robot.h"
28
+//#include "image.h"
29
+//#include "message.h"
30
+//#include "server.h"
62 31
 
63
-extern int MSG_QUEUE_SIZE;
32
+#include "camera.h"
33
+#include "img.h"
64 34
 
65
-extern int PRIORITY_TSERVER;
66
-extern int PRIORITY_TOPENCOMROBOT;
67
-extern int PRIORITY_TMOVE;
68
-extern int PRIORITY_TSENDTOMON;
69
-extern int PRIORITY_TRECEIVEFROMMON;
70
-extern int PRIORITY_TSTARTROBOT;
35
+#include "messages.h"
36
+#include "commonitor.h"
37
+#include "comrobot.h"
71 38
 
72
-/**
73
- * \brief       Thread handling server communication.
74
- */ 
75
-void f_server(void *arg);
76
-
77
-/**
78
- * \brief       Thread handling communication to monitor.
79
- */ 
80
-void f_sendToMon(void *arg);
81
-
82
-/**
83
- * \brief       Thread handling communication from monitor.
84
- */ 
85
-void f_receiveFromMon(void *arg);
86
-
87
-/**
88
- * \brief       Thread handling opening of robot communication.
89
- */ 
90
-void f_openComRobot(void * arg);
91
-
92
-/**
93
- * \brief       Thread handling robot mouvements.
94
- */ 
95
-void f_move(void *arg);
39
+#include <thread>
40
+#include <mutex>
41
+#include <condition_variable>
96 42
 
43
+class Tasks {
44
+public:
97 45
 /**
98
- * \brief       Thread handling robot activation.
99
- */ 
100
-void f_startRobot(void *arg);
46
+     * @brief Initialisation des structures de l'application (tâches, mutex, 
47
+     * semaphore, etc.)
48
+     */
49
+    void Init();
50
+
51
+    /**
52
+     * @brief Démarrage des tâches
53
+     */
54
+    void Run();
55
+
56
+    /**
57
+     * @brief Arrêt des tâches
58
+     */
59
+    void Stop();
60
+    
61
+    /**
62
+     */
63
+    void Join() {
64
+        threadServer->join();
65
+        threadTimer->join();
66
+        threadSendToMon->join();
67
+    }
68
+    
69
+    /**
70
+     */
71
+    bool AcceptClient() {
72
+        return monitor.AcceptClient();
73
+    }
74
+    
75
+     /**
76
+     * @brief Thread handling server communication.
77
+     */
78
+    void ServerTask(void *arg);
79
+
80
+    /**
81
+     * @brief Thread handling server communication.
82
+     */
83
+    void TimerTask(void *arg);
84
+    
85
+    /**
86
+     * @brief Thread handling communication to monitor.
87
+     */
88
+    void SendToMonTask(void *arg);
89
+private:
90
+    ComMonitor monitor;
91
+    ComRobot robot;
92
+    
93
+    thread *threadServer;
94
+    thread *threadSendToMon;
95
+    thread *threadTimer;
96
+//    thread *threadReceiveFromMon;
97
+//    thread *threadOpenComRobot;
98
+//    thread *threadStartRobot;
99
+//    thread *threadMove;
100
+//    thread *threadTimer;
101
+    
102
+    mutex mutexTimer;
103
+//    mutex mutexRobotStarted;
104
+//    mutex mutexMove;
105
+//    mutex semBarrier;
106
+//    mutex semOpenComRobot;
107
+//    mutex semServerOk;
108
+//    mutex semStartRobot;
109
+
110
+    
111
+//
112
+//    /**
113
+//     * @brief Thread handling communication from monitor.
114
+//     */
115
+//    void ReceiveFromMonTask(void *arg);
116
+//
117
+//    /**
118
+//     * @brief Thread handling opening of robot communication.
119
+//     */
120
+//    void OpenComRobotTask(void * arg);
121
+//
122
+//    /**
123
+//     * @brief Thread handling robot mouvements.
124
+//     */
125
+//    void MoveTask(void *arg);
126
+//
127
+//    /**
128
+//     * @brief Thread handling robot activation.
129
+//     */
130
+//    void StartRobotTask(void *arg);
131
+};
101 132
 
102 133
 #endif // __WITH_PTHREAD__
103 134
 #endif /* __TASKS_H__ */

+ 1
- 1
software/robot/src/cmdManager.c View File

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

Loading…
Cancel
Save