|
@@ -26,6 +26,7 @@
|
26
|
26
|
#define PRIORITY_TRECEIVEFROMMON 25
|
27
|
27
|
#define PRIORITY_TSTARTROBOT 20
|
28
|
28
|
#define PRIORITY_TCAMERA 21
|
|
29
|
+#define PRIORITY_TBATTERY 31
|
29
|
30
|
|
30
|
31
|
/*
|
31
|
32
|
* Some remarks:
|
|
@@ -123,6 +124,10 @@ void Tasks::Init() {
|
123
|
124
|
cerr << "Error task create: " << strerror(-err) << endl << flush;
|
124
|
125
|
exit(EXIT_FAILURE);
|
125
|
126
|
}
|
|
127
|
+ if (err = rt_task_create(&th_getBattery, "th_getBattery", 0, PRIORITY_TBATTERY, 0)) {
|
|
128
|
+ cerr << "Error task create: " << strerror(-err) << endl << flush;
|
|
129
|
+ exit(EXIT_FAILURE);
|
|
130
|
+ }
|
126
|
131
|
cout << "Tasks created successfully" << endl << flush;
|
127
|
132
|
|
128
|
133
|
/**************************************************************************************/
|
|
@@ -167,7 +172,11 @@ void Tasks::Run() {
|
167
|
172
|
cerr << "Error task start: " << strerror(-err) << endl << flush;
|
168
|
173
|
exit(EXIT_FAILURE);
|
169
|
174
|
}
|
170
|
|
-
|
|
175
|
+ if (err = rt_task_start(&th_getBattery, (void(*)(void*)) & Tasks::ReadBattery, this)) {
|
|
176
|
+ cerr << "Error task start: " << strerror(-err) << endl << flush;
|
|
177
|
+ exit(EXIT_FAILURE);
|
|
178
|
+ }
|
|
179
|
+
|
171
|
180
|
cout << "Tasks launched" << endl << flush;
|
172
|
181
|
}
|
173
|
182
|
|
|
@@ -351,6 +360,7 @@ void Tasks::StartRobotTask(void *arg) {
|
351
|
360
|
*/
|
352
|
361
|
void Tasks::MoveTask(void *arg) {
|
353
|
362
|
int rs;
|
|
363
|
+ int previousMove = MESSAGE_ROBOT_GO_FORWARD;
|
354
|
364
|
int cpMove;
|
355
|
365
|
|
356
|
366
|
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
|
|
@@ -372,12 +382,15 @@ void Tasks::MoveTask(void *arg) {
|
372
|
382
|
rt_mutex_acquire(&mutex_move, TM_INFINITE);
|
373
|
383
|
cpMove = move;
|
374
|
384
|
rt_mutex_release(&mutex_move);
|
375
|
|
-
|
376
|
|
- cout << " move: " << cpMove;
|
377
|
|
-
|
378
|
|
- rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
379
|
|
- robot.Write(new Message((MessageID)cpMove));
|
380
|
|
- rt_mutex_release(&mutex_robot);
|
|
385
|
+ if (cpMove != previousMove) {
|
|
386
|
+ cout << " move: " << cpMove;
|
|
387
|
+
|
|
388
|
+ rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
|
389
|
+ robot.Write(new Message((MessageID)cpMove));
|
|
390
|
+ rt_mutex_release(&mutex_robot);
|
|
391
|
+
|
|
392
|
+ previousMove = cpMove;
|
|
393
|
+ }
|
381
|
394
|
}
|
382
|
395
|
cout << endl << flush;
|
383
|
396
|
}
|
|
@@ -415,3 +428,34 @@ Message *Tasks::ReadInQueue(RT_QUEUE *queue) {
|
415
|
428
|
return msg;
|
416
|
429
|
}
|
417
|
430
|
|
|
431
|
+void Tasks::ReadBattery(void *arg){
|
|
432
|
+ cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
|
|
433
|
+ // Synchronization barrier (waiting that all tasks are starting)
|
|
434
|
+ rt_sem_p(&sem_barrier, TM_INFINITE);
|
|
435
|
+
|
|
436
|
+ /**************************************************************************************/
|
|
437
|
+ /* The task starts here */
|
|
438
|
+ /**************************************************************************************/
|
|
439
|
+ rt_task_set_periodic(NULL, TM_NOW, 500000000);
|
|
440
|
+
|
|
441
|
+ int rs;
|
|
442
|
+
|
|
443
|
+ while (1) {
|
|
444
|
+ rt_task_wait_period(NULL);
|
|
445
|
+ cout << "Periodic battery get lvl \n";
|
|
446
|
+ rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
|
447
|
+ rs = robotStarted;
|
|
448
|
+ rt_mutex_release(&mutex_robotStarted);
|
|
449
|
+ if (rs == 1) {
|
|
450
|
+ rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
|
451
|
+ Message *msg = robot.Write( robot.GetBattery() ) ;
|
|
452
|
+ rt_mutex_release(&mutex_robot);
|
|
453
|
+ rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
|
|
454
|
+ monitor.Write(msg);
|
|
455
|
+ rt_mutex_release(&mutex_monitor);
|
|
456
|
+
|
|
457
|
+ }
|
|
458
|
+
|
|
459
|
+ cout << endl << flush;
|
|
460
|
+ }
|
|
461
|
+}
|