diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 9375fd7..0fa457d 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -90,7 +90,7 @@ void Tasks::Init() { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } - if (err = rt_mutex_create(&mutex_arenaList, NULL)) { + if (err = rt_mutex_create(&mutex_arena, NULL)) { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } @@ -153,7 +153,7 @@ void Tasks::Init() { cerr << "Error task create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } - if (err = rt_task_create(&th_cameraOpen, "th_cameraOpen", 0, PRIORITY_TSETCAMERA, 0)) { + if (err = rt_task_create(&th_cameraManage, "th_cameraManage", 0, PRIORITY_TSETCAMERA, 0)) { cerr << "Error task create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } @@ -161,10 +161,6 @@ void Tasks::Init() { cerr << "Error task create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } - if (err = rt_task_create(&th_cameraClose, "th_cameraClose", 0, PRIORITY_TSETCAMERA, 0)) { - cerr << "Error task create: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } if (err = rt_task_create(&th_arenaChoice, "th_arenaChoice", 0, PRIOTITY_TARENA, 0)) { cerr << "Error task create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); @@ -218,7 +214,7 @@ void Tasks::Run() { cerr << "Error task start: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } - if (err = rt_task_start(&th_cameraOpen, (void(*)(void*)) & Tasks::OpenCamera, this)) { + if (err = rt_task_start(&th_cameraManage, (void(*)(void*)) & Tasks::ManageCamera, this)) { cerr << "Error task start: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } @@ -226,10 +222,6 @@ void Tasks::Run() { cerr << "Error task start: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } - if (err = rt_task_start(&th_cameraClose, (void(*)(void*)) & Tasks::CloseCamera, this)) { - cerr << "Error task start: " << strerror(-err) << endl << flush; - exit(EXIT_FAILURE); - } if (err = rt_task_start(&th_arenaChoice, (void(*)(void*)) & Tasks::ArenaChoice, this)) { cerr << "Error task start: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); @@ -370,14 +362,14 @@ void Tasks::ReceiveFromMonTask(void *arg) { else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) { std::cout << "\n\n\nArena Confirm asked !!!\n\n\n" << std::endl; rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE); - if(cameraStatus == ArenaStatusEnum::SEARCHING) + if(cameraStatus == ArenaStatusEnum::SEARCHED) cameraStatus = ArenaStatusEnum::CONFIRM; rt_mutex_release(&mutex_arenaStatus); } else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) { std::cout << "\n\n\nArena Infirm asked !!!\n\n\n" << std::endl; rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE); - if(cameraStatus == ArenaStatusEnum::SEARCHING) + if(cameraStatus == ArenaStatusEnum::SEARCHED) cameraStatus = ArenaStatusEnum::INFIRM; rt_mutex_release(&mutex_arenaStatus); } @@ -561,7 +553,7 @@ void Tasks::BatteryStatusTask(void * arg) { } } -void Tasks::OpenCamera(void * arg) +void Tasks::ManageCamera(void * arg) { // Variables int rs(0); @@ -582,27 +574,24 @@ void Tasks::OpenCamera(void * arg) rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rs = robotStarted; rt_mutex_release(&mutex_robotStarted); - - // Check the status of the camera - rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE); - cs = cameraStatus; - rt_mutex_release(&mutex_cameraStatus); - if((rs != 0) && (cs == CameraStatusEnum::OPENING)) + + if(rs != 0) { - rt_mutex_acquire(&mutex_camera, TM_INFINITE); - cam = new Camera(sm, 10); - bool is_opened = cam->Open(); - rt_mutex_release(&mutex_camera); + // Check the status of the camera rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE); - if(is_opened) + cs = cameraStatus; + rt_mutex_release(&mutex_cameraStatus); + + if(cs == CameraStatusEnum::OPENING) { - cameraStatus = CameraStatusEnum::OPENED; + const MessageID tempMessage = this->OpenCamera(); + WriteInQueue(&q_messageToMon, new Message(tempMessage)); + } + if(cs == CameraStatusEnum::CLOSING) + { + this->CloseCamera(); WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); } - else { - WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK)); - } - rt_mutex_release(&mutex_cameraStatus); } } } @@ -621,62 +610,73 @@ void Tasks::ImageCamera(void * arg) while(1) { rt_task_wait_period(NULL); + // Check the status of the camera rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE); cs = cameraStatus; rt_mutex_release(&mutex_cameraStatus); + + // Checking if camera is opened first if(cs == CameraStatusEnum::OPENED) { + // Gathering image from camera Img * img = new Img(cam->Grab()); + + // Sending Image MessageImg *msgImg = new MessageImg(MESSAGE_CAM_IMAGE, img); rt_mutex_acquire(&mutex_monitor, TM_INFINITE); monitor.Write(msgImg); rt_mutex_release(&mutex_monitor); + // ? delete img; } } } -void Tasks::CloseCamera(void * arg) +MessageID Tasks::OpenCamera() { - // Variables - CameraStatusEnum cs(CameraStatusEnum::CLOSED); - - cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; - // Synchronization barrier (waiting that all tasks are starting) - rt_sem_p(&sem_barrier, TM_INFINITE); + cout << "Called " << __PRETTY_FUNCTION__ << endl << flush; + // Opening camera + rt_mutex_acquire(&mutex_camera, TM_INFINITE); + cam = new Camera(sm, 10); + const bool isOpened = cam->Open(); + rt_mutex_release(&mutex_camera); - rt_task_set_periodic(NULL, TM_NOW, 500000000); - - while(1) + // Changing status + if(isOpened) { - rt_task_wait_period(NULL); - - // Check the status of the camera rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE); - cs = cameraStatus; + cameraStatus = CameraStatusEnum::OPENED; rt_mutex_release(&mutex_cameraStatus); - if(cs == CameraStatusEnum::CLOSING) - { - rt_mutex_acquire(&mutex_camera, TM_INFINITE); - cam->Close(); - delete cam; - rt_mutex_release(&mutex_camera); - rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE); - cameraStatus = CameraStatusEnum::CLOSED; - rt_mutex_release(&mutex_cameraStatus); - WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); - } } + return (is_opened ? MESSAGE_ANSWER_ACK : MESSAGE_ANSWER_NACK); +} + +void Tasks::CloseCamera() +{ + cout << "Called " << __PRETTY_FUNCTION__ << endl << flush; + // Closing Camera + rt_mutex_acquire(&mutex_camera, TM_INFINITE); + cam->Close(); + delete cam; + rt_mutex_release(&mutex_camera); + + // Changing Status + rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE); + cameraStatus = CameraStatusEnum::CLOSED; + rt_mutex_release(&mutex_cameraStatus); } void Tasks::ArenaChoice(void * arg) { cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); + + // Variables Img * img = nullptr; Arena a; - ArenaStatusEnum as = ArenaStatusEnum::NONE; + ArenaStatusEnum as(ArenaStatusEnum::NONE); while(1) { @@ -688,14 +688,16 @@ void Tasks::ArenaChoice(void * arg) // ASK_ARENA if(as == ArenaStatusEnum::SEARCHING) { + rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE); + const CameraStatusEnum cs = cameraStatus; + rt_mutex_release(&mutex_cameraStatus); + // Gathering last image + closing camera when prompted - rt_mutex_acquire(&mutex_camera, TM_INFINITE); - if(cam->IsOpen()) + if(CameraStatusEnum::OPENED == cs) { - img = new Img(cam->Grab()); - cam->Close(); + new Img(cam->Grab()); + this->CloseCamera(); } - rt_mutex_release(&mutex_camera); // Putting the arena overlay if found a = img->SearchArena(); @@ -708,24 +710,34 @@ void Tasks::ArenaChoice(void * arg) monitor.Write(msgImg); rt_mutex_release(&mutex_monitor); } + // ? delete img; } // ARENA_CONFIRM / INFIRM - else if(as != ArenaStatusEnum::NONE) + else if(as == ArenaStatusEnum::CONFIRM || as == ArenaStatusEnum::INFIRM) { + // Store arena to internal buffer + change status if(as == ArenaStatusEnum::CONFIRM) { - rt_mutex_acquire(&mutex_arenaList, TM_INFINITE); - arenaList.emplace_back(a); - rt_mutex_release(&mutex_arenaList); + rt_mutex_acquire(&mutex_arena, TM_INFINITE); + arena = a; + rt_mutex_release(&mutex_arena); + rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE); + arenaStatus = ArenaStatusEnum::CONFIRMED; + rt_mutex_release(&mutex_arenaStatus); } - // Re-open the camera - rt_mutex_acquire(&mutex_camera, TM_INFINITE); - cam->Open(); - rt_mutex_release(&mutex_camera); - // empty the arena and write ACK to the monitor + // Arena Status set to NONE to enable new search + else { + rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE); + arenaStatus = ArenaStatusEnum::NONE; + rt_mutex_release(&mutex_arenaStatus); + } + + // empty the temporary arena a = Arena(); - WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); + // Re-open the camera + const MessageID tempMessage = this->OpenCamera(); + WriteInQueue(&q_messageToMon, new Message(tempMessage)); } } } \ No newline at end of file diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index 823cec5..1274ba5 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -47,7 +47,9 @@ enum class CameraStatusEnum { enum class ArenaStatusEnum { NONE, SEARCHING, + SEARCHED, CONFIRM, + CONFIRMED, INFIRM }; @@ -84,7 +86,7 @@ private: bool robotBatteryGet = false; CameraStatusEnum cameraStatus = CameraStatusEnum::CLOSED; ArenaStatusEnum arenaStatus = ArenaStatusEnum::NONE; - std::vector arenaList; + Arena arena; Camera* cam = nullptr; /**********************************************************************/ @@ -97,9 +99,8 @@ private: RT_TASK th_startRobot; RT_TASK th_move; RT_TASK th_battery; - RT_TASK th_cameraOpen; + RT_TASK th_cameraManage; RT_TASK th_cameraImage; - RT_TASK th_cameraClose; RT_TASK th_arenaChoice; /**********************************************************************/ @@ -112,7 +113,7 @@ private: RT_MUTEX mutex_batteryGet; RT_MUTEX mutex_cameraStatus; RT_MUTEX mutex_camera; - RT_MUTEX mutex_arenaList; + RT_MUTEX mutex_arena; RT_MUTEX mutex_arenaStatus; /**********************************************************************/ @@ -164,14 +165,15 @@ private: /* OUR CODE */ - + + // Threads void BatteryStatusTask(void * arg); - void OpenCamera(void * arg); + void ManageCamera(void * arg); void ImageCamera(void * arg); - void CloseCamera(void * arg); void ArenaChoice(void * arg); // Utility functions - + MessageID OpenCamera(); + void CloseCamera(); /**********************************************************************/