|
@@ -123,6 +123,10 @@ void Tasks::Init() {
|
123
|
123
|
cerr << "Error task create: " << strerror(-err) << endl << flush;
|
124
|
124
|
exit(EXIT_FAILURE);
|
125
|
125
|
}
|
|
126
|
+ if (err = rt_task_create(&th_battery, "th_battery", 0, PRIORITY_TMOVE, 0)) {
|
|
127
|
+ cerr << "Error task create: " << strerror(-err) << endl << flush;
|
|
128
|
+ exit(EXIT_FAILURE);
|
|
129
|
+ }
|
126
|
130
|
cout << "Tasks created successfully" << endl << flush;
|
127
|
131
|
|
128
|
132
|
/**************************************************************************************/
|
|
@@ -167,6 +171,10 @@ void Tasks::Run() {
|
167
|
171
|
cerr << "Error task start: " << strerror(-err) << endl << flush;
|
168
|
172
|
exit(EXIT_FAILURE);
|
169
|
173
|
}
|
|
174
|
+ if (err = rt_task_start(&th_battery, (void(*)(void*)) & Tasks::SendBattery, this)) {
|
|
175
|
+ cerr << "Error task start: " << strerror(-err) << endl << flush;
|
|
176
|
+ exit(EXIT_FAILURE);
|
|
177
|
+ }
|
170
|
178
|
|
171
|
179
|
cout << "Tasks launched" << endl << flush;
|
172
|
180
|
}
|
|
@@ -352,6 +360,7 @@ void Tasks::StartRobotTask(void *arg) {
|
352
|
360
|
void Tasks::MoveTask(void *arg) {
|
353
|
361
|
int rs;
|
354
|
362
|
int cpMove;
|
|
363
|
+ int previousMove = -1;
|
355
|
364
|
|
356
|
365
|
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
|
357
|
366
|
// Synchronization barrier (waiting that all tasks are starting)
|
|
@@ -369,15 +378,18 @@ void Tasks::MoveTask(void *arg) {
|
369
|
378
|
rs = robotStarted;
|
370
|
379
|
rt_mutex_release(&mutex_robotStarted);
|
371
|
380
|
if (rs == 1) {
|
372
|
|
- rt_mutex_acquire(&mutex_move, TM_INFINITE);
|
373
|
|
- cpMove = move;
|
374
|
|
- 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);
|
|
381
|
+ if (!(previousMove == move)){
|
|
382
|
+ rt_mutex_acquire(&mutex_move, TM_INFINITE);
|
|
383
|
+ cpMove = move;
|
|
384
|
+ rt_mutex_release(&mutex_move);
|
|
385
|
+
|
|
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
|
+ previousMove = cpMove;
|
|
392
|
+ }
|
381
|
393
|
}
|
382
|
394
|
cout << endl << flush;
|
383
|
395
|
}
|
|
@@ -415,3 +427,37 @@ Message *Tasks::ReadInQueue(RT_QUEUE *queue) {
|
415
|
427
|
return msg;
|
416
|
428
|
}
|
417
|
429
|
|
|
430
|
+void Tasks::SendBattery(void *arg){
|
|
431
|
+ int rs;
|
|
432
|
+ Message * responseBattery;
|
|
433
|
+
|
|
434
|
+ cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
|
|
435
|
+ // Synchronization barrier (waiting that all tasks are starting)
|
|
436
|
+ rt_sem_p(&sem_barrier, TM_INFINITE);
|
|
437
|
+
|
|
438
|
+ /**************************************************************************************/
|
|
439
|
+ /* The task starts here */
|
|
440
|
+ /**************************************************************************************/
|
|
441
|
+ rt_task_set_periodic(NULL, TM_NOW, 500000000);
|
|
442
|
+
|
|
443
|
+ while(1){
|
|
444
|
+ rt_task_wait_period(NULL);
|
|
445
|
+ rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
|
446
|
+ rs = robotStarted;
|
|
447
|
+ rt_mutex_release(&mutex_robotStarted);
|
|
448
|
+ cout << "Periodic battery update before rs" << endl << flush;
|
|
449
|
+ if (rs){
|
|
450
|
+ cout << "Periodic battery update" << endl << flush;
|
|
451
|
+
|
|
452
|
+ rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
|
453
|
+ responseBattery = robot.Write(new Message(MESSAGE_ROBOT_BATTERY_GET));
|
|
454
|
+ rt_mutex_release(&mutex_robot);
|
|
455
|
+
|
|
456
|
+ cout << "Periodic battery update before comparing message" << endl << flush;
|
|
457
|
+ if (responseBattery->CompareID(MESSAGE_ROBOT_BATTERY_LEVEL)){
|
|
458
|
+ WriteInQueue(&q_messageToMon,responseBattery);
|
|
459
|
+ }
|
|
460
|
+ }
|
|
461
|
+ cout << endl << flush;
|
|
462
|
+ }
|
|
463
|
+}
|