real_time/software/raspberry/superviseur-robot/example/rtvideoExample.cpp
Sébastien DI MERCURIO 776a2f4ce6 initial commit
2018-08-27 16:39:49 +02:00

89 lines
2 KiB
C++

#include <task.h>
#include "../src/imagerie.h"
#include "../src/serial.h"
#include "../src/tcpServer.h" // include himself imagerie.h
using namespace std;
using namespace cv;
using namespace raspicam;
RT_TASK video;
void threadVideo(void *arg)
{
printf("Thread lancé ... \n");
Camera rpiCam;
Image imgVideo;
Arene monArene;
position positionRobots[20];
Jpg compress;
openCamera(&rpiCam);
do
{
getImg(&rpiCam, &imgVideo);
if(detectArena(&imgVideo, &monArene)==0)
{
detectPosition(&imgVideo,positionRobots,&monArene);
drawArena(&imgVideo,&imgVideo,&monArene);
}
else
detectPosition(&imgVideo,positionRobots);
drawPosition(&imgVideo,&imgVideo,&positionRobots[0]);
imgCompress(&imgVideo,&compress);
sendToUI("IMG",&compress);
sendToUI("POS",&positionRobots[0]);
}while(waitKey(30)!='q');
closeCam(&rpiCam);
}
int main() {
serverOpen();
robotOpenCom();
char header[4];
char data[20];
memset(data, '\0',20);
memset(header,'\0',4);
if(rt_task_spawn(&video,"envoieVideo",0,20,0, threadVideo, NULL) == -1)
perror("erreur lors de la creation du thread\n");
do
{ receptionFromUI(header,data);
if(strcmp(header, DMB) == 0)
{
printf("EVENEMENT DUMBER DETECTE AVEC LE MESSAGE :%s \n",data);
int a = robotCmd(data[0]);
printf("Resultat CMD : %d \n", a);
if(data[0] == 'u' && a == 0)
{
sendToUI(ACK);
}
if(data[0] == 'r' && a == 0)
{
sendToUI(ACK);
}
}
if(strcmp(header, MES) == 0)
{
printf("EVENEMENT MESSAGE DETECTE AVEC LE MESSAGE :%s \n",data);
}
if(strcmp(header,POS)==0)
{
printf("EVENEMENT POSITION DETECTE AVEC LE MESSAGE :%s \n",data);
}
}while((strcmp(header,MES)!=0) || (data[0] != 'C'));
robotCloseCom();
serverClose();
return 0;
}