INSA/dumber
1
0
Fork 0
mirror of https://github.com/yoboujon/dumber.git synced 2025-06-08 05:40:49 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Camera: Added boilerplate for camera opening and image gathering (to test). Battery: Sending data to monitor.

Browse source
This commit is contained in:
Yohan Boujon 2024-03-26 23:17:38 +01:00
parent 7d8d4456aa
commit 8d8dc0586f
2 changed files with 119 additions and 5 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

110
software/raspberry/superviseur-robot/tasks.cpp
View file

@ -74,10 +74,19 @@ void Tasks::Init() {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
// Added Mutexes
if (err = rt_mutex_create(&mutex_batteryGet, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_cameraStatus, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_camera, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Mutexes created successfully" << endl << flush;
/**************************************************************************************/
@ -128,10 +137,19 @@ void Tasks::Init() {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
// Added tasks
if (err = rt_task_create(&th_battery, "th_battery", 0, PRIORITY_TBATTERY, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_cameraOpen, "th_cameraOpen", 0, PRIORITY_TCAMERA, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_cameraImage, "th_cameraImage", 0, PRIORITY_TCAMERA, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Tasks created successfully" << endl << flush;
/**************************************************************************************/
@ -176,10 +194,19 @@ void Tasks::Run() {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
// Added Tasks
if (err = rt_task_start(&th_battery, (void(*)(void*)) & Tasks::BatteryStatusTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_cameraOpen, (void(*)(void*)) & Tasks::OpenCamera, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_cameraImage, (void(*)(void*)) & Tasks::ImageCamera, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Tasks launched" << endl << flush;
}
@ -277,10 +304,6 @@ void Tasks::ReceiveFromMonTask(void *arg) {
exit(-1);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
rt_sem_v(&sem_openComRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_BATTERY_GET)) {
rt_mutex_acquire(&mutex_batteryGet, TM_INFINITE);
robotBatteryGet = true;
rt_mutex_release(&mutex_batteryGet);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
rt_sem_v(&sem_startRobot);
} else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
@ -293,6 +316,21 @@ void Tasks::ReceiveFromMonTask(void *arg) {
move = msgRcv->GetID();
rt_mutex_release(&mutex_move);
}
// Added messages
else if (msgRcv->CompareID(MESSAGE_ROBOT_BATTERY_GET)) {
rt_mutex_acquire(&mutex_batteryGet, TM_INFINITE);
robotBatteryGet = true;
rt_mutex_release(&mutex_batteryGet);
}
else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
if(cameraStatus != CameraStatusEnum::OPENED)
cameraStatus = CameraStatusEnum::OPENING;
rt_mutex_release(&mutex_cameraStatus);
}
else if (msgRcv->CompareID(MESSAGE_CAM_IMAGE)) {
//?
}
delete(msgRcv); // mus be deleted manually, no consumer
}
}
@ -462,7 +500,10 @@ void Tasks::BatteryStatusTask(void * arg) {
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
Message* msg = robot.Write(new Message(MESSAGE_ROBOT_BATTERY_GET));
rt_mutex_release(&mutex_robot);
std::cout << "Message Received: " << msg->ToString() << std::endl;
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
monitor.Write(msg);
rt_mutex_release(&mutex_monitor);
robotBatteryGet = false;
getbatterytask = false;
@ -470,3 +511,62 @@ void Tasks::BatteryStatusTask(void * arg) {
rt_mutex_release(&mutex_batteryGet);
}
}
void Tasks::OpenCamera(void * arg)
{
// Variables
int rs(0);
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);
while(1)
{
// Verify that the robot has started
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))
{
rt_mutex_acquire(&mutex_camera, TM_INFINITE);
cam = new Camera(sm, 10);
rt_mutex_release(&mutex_camera);
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
cameraStatus = CameraStatusEnum::OPENED;
rt_mutex_release(&mutex_cameraStatus);
}
}
}
void Tasks::ImageCamera(void * arg)
{
// 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);
while(1)
{
// Check the status of the camera
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
cs = cameraStatus;
rt_mutex_release(&mutex_cameraStatus);
if(cs == CameraStatusEnum::OPENED)
{
Img * img = new Img(cam->Grab());
MessageImg *msgImg = new MessageImg(MESSAGE_CAM_IMAGE, img);
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
monitor.Write(msgImg);
rt_mutex_release(&mutex_monitor);
}
}
}

14
software/raspberry/superviseur-robot/tasks.h
View file

@ -36,6 +36,12 @@
using namespace std;
enum class CameraStatusEnum {
CLOSED,
OPENING,
OPENED
};
class Tasks {
public:
/**
@ -67,6 +73,8 @@ private:
int robotStarted = 0;
int move = MESSAGE_ROBOT_STOP;
bool robotBatteryGet = false;
CameraStatusEnum cameraStatus = CameraStatusEnum::CLOSED;
Camera* cam = nullptr;
/**********************************************************************/
/* Tasks */
@ -78,6 +86,8 @@ private:
RT_TASK th_startRobot;
RT_TASK th_move;
RT_TASK th_battery;
RT_TASK th_cameraOpen;
RT_TASK th_cameraImage;
/**********************************************************************/
/* Mutex */
@ -87,6 +97,8 @@ private:
RT_MUTEX mutex_robotStarted;
RT_MUTEX mutex_move;
RT_MUTEX mutex_batteryGet;
RT_MUTEX mutex_cameraStatus;
RT_MUTEX mutex_camera;
/**********************************************************************/
/* Semaphores */
@ -139,6 +151,8 @@ private:
/* OUR CODE */
void BatteryStatusTask(void * arg);
void OpenCamera(void * arg);
void ImageCamera(void * arg);
/**********************************************************************/