книги / Прототипирование сетевой системы управления. Разработка Windows-приложения удаленного контроллера прототипа робота-официанта на базе PROMOBOT V

На карту можно поместить несколько точек, называемых ориентирами: навести курсор в нужную точку и нажать правую кнопку.

Для запоминания карты нажмите кнопку «Write File» диалогового окна. Для контроля нажмите кнопку «Read File», появится только что созданная карта лаборатории с ориентирами (рис. 4.9).

Рис. 4.9. Карта лаборатории с препятствиями и ориентиром

4.3.Разработка клиента навигационной системы

1.Поместить на окно главной формы (ресурсный файл main.rc) три элемента Text с идентификаторами IDGPSX, IDGPSY и IDGPST для отображения текущих значений координат мобильного маяка и времени работы маяка.

2.Открыть UDP-порт 0xabce для взаимодействия с сервером навигационной системы. Для этого необходимо вызвать функцию OpenUdpSocet в MainDlgProc в месте обработки сообщения WM_INITDIALOG, предназначенном для инициализации про-

21

граммы (как в п. 4.3.2, п. 4 (г) лабораторной работы № 3), в которой аргумент со значением 1 означает, что порту присваивается порядковый номер 1:

OpenUdpSocet(0xabce,DataProcessingGPS,1);

Открытый ранее порт с порядковым номером 0 – это порт для взаимодействия с промобот-сервером.

Сделать символическое определение адреса мобильного маяка (в примере 6):

#define HEDGEHOG 6

Объявить глобальные переменные для сохранения координат мобильного маяка и времени работы маяка:

static int X,Y; static UINT32 T;

Написать начальный вариант функции обработки данных DataProcessingGPS ответа сервера навигационной системы (dash- board-сервера), приходящего на порт 0xabce. Формат ответа приведен в табл. 4.1.

22

void DataProcessingGPS(UCHAR *sbuf,DWORD NumberOfBytes, struct sockaddr_in sin, int len)

{

char szBuffer[128]; INT16 xx,yy;

float d; POINT p;

xx=*( (INT16*)(&sbuf[9])); yy=*( (INT16*)(&sbuf[11])); if (sbuf[0]==HEDGEHOG){

X=xx;

Y=yy; sprintf(szBuffer,"X=%d ",X);

SetDlgItemText(ghwndDlg,IDGPSX,szBuffer); sprintf(szBuffer,"Y=%d ",Y); SetDlgItemText(ghwndDlg,IDGPSY,szBuffer);

}

T=*( (UINT32*)(&sbuf[5])); sprintf(szBuffer,"T=%d ",T); SetDlgItemText(ghwndDlg,IDGPST,szBuffer);

}

3. Написать функции запроса SendToGPS координат мобильного маяка. Формат запроса приведены в табл. 4.2.

23

Поместить на окно главной формы (ресурсный файл main.rc) элемент Editbox с идентификаторами IDIPGPS для задания значения IP-адреса dashboard-сервера. Из этого элемента будем считывать адрес в функции SendToGPS:

static void SendToGPS(void){ struct sockaddr_in sin; char Text[17];

UINT8 sbuf[MAX_SBUF]; sbuf[0]=HEDGEHOG; sbuf[1]=0x47; sbuf[2]= 1;

sbuf[3]=0;

sbuf[4]=4;

sbuf[5]=0;

sbuf[6]=0;

sbuf[7]=0x10;

sbuf[8]=0;

sbuf[9]=0;

sin.sin_family = AF_INET; GetDlgItemText(ghwndDlg, IDIPGPS,Text,17); sin.sin_addr.s_addr=inet_addr(Text); sin.sin_port= htons(48812);

if (sendto(SIdUdp[1],(const char *)sbuf,10,0,(struct sockaddr *)&sin, sizeof(sin))<0)

SendMessageList("Socet 48812: Can't sendto");

}

Сделать символическое определение периода опроса (125 ms) dashboard-сервера:

#define GPS_POLLING_PERIOD 125

В разделе инициализации (WM_INITDIALOG) создать тай-

мер 7 с временем GPS_POLLING_PERIOD:

SetTimer(hwndDlg, 7,GPS_POLLING_PERIOD,NULL);

В разделе обработки сообщений WM_TIMER для таймера 7 вызывать функцию SendToGPS:

24

case 7:

SendToGPS();

break;

После запуска программы установить IP-адрес dashboardсервера и наблюдать за изменением значений в элементах диалогового интерфейса с идентификаторами IDGPSX, IDGPSY и IDGPST.

4.4.Чтение карты помещения

1.Поместить на окно главной формы (ресурсный файл main.rc) кнопку (элемент Pushbutton) с идентификатором READ_FILE для запуска чтения файла с картой помещения.

Карта помещения представляется в виде множества полигонов, описывающих контуры стен, препятствий и координаты ориентиров.

Сделать символическое определение констант:

Первая константа задает максимальное количество полигонов, вторая – максимальное количество узлов (углов) в полигонах, а третья – максимальное количество ориентиров.

2. Объявить тип данных FloorPlanDB312_t, представляющий карту помещения:

typedef struct { UINT16 PoligonNum;

UINT16 PoligonNodesNum[OBSTACALES_MAX]; POINT

Poligons[OBSTACALES_MAX][POLIGON_NODES_MAX]; UINT16 LandMarksNum;

POINT LandMarks[LAND_MARKS_MAX]; POINT X0_Y0;

INT16 X0Y0_OFFSET; POINT XM_YM;

UINT16 MarvelmindXazimuth; } FloorPlanDB312_t;

25

Элемент PoligonNum сохраняет общее количество полигонов карты.

Элемент PoligonNodesNum – массив, i-й элемент которого сохраняет количество узлов полигона i.

Элемент Poligons – двумерный массив, [i, j] элемент которого сохраняет координаты узла j полигона i.

Элемент LandMarksNum сохраняет общее количество ориентиров.

Элемент LandMarks[LAND_MARKS_MAX] – массив, i-й эле-

мент которого сохраняет координаты ориентира i.

Элементы XM_YM и X0_Y0 сохраняют координаты точек, представленных в п. 4.2.1, для связывания карт навигационной системы и помещения.

Элемент X0Y0_OFFSET принимает значение 0.

Элемент MarvelmindXazimuth сохраняет азимут оси Х карты на случай использования компаса для ориентации.

Объявить глобальную переменную, сохраняющую карту лаборатории и глобальные переменные, сохраняющие координаты точек, используемых при:

static FloorPlanDB312_t FloorPlan; static POINT X0_Y0;

static POINT XM_YM; static INT16 X0Y0_OFFSET;

В разделе инициализации (WM_INITDIALOG) присвоить начальное значение:

FloorPlan.PoligonNum=0;

3. Объявить константу, определяющую цвет фона:

#define FON_COLOR RGB(220,220,220)

Написать функцию рисования карта помещения DrawFloor. В этой функции перебираются все полигоны. Контуры полигонов рисуются черным цветом. Нулевой полигон (контур стен) заполняется фоновым цветом, а полигоны-препятствия – красным. Ориентиры рисуются укрупненными черными точками.

26

status = 1; if (status != 0 ){

SendMessageList("ReadMyFile: Can't CreateFile"); }else{

file_size=GetFileSize(hFile,NULL); *file_len=file_size; if(file_size==0xffffffff){

SendMessageList("ReadMyFile: Can't GetFileSize"); }else{

if (FALSE==ReadFile(hFile,&FloorPlan,sizeof(FloorPlan), &NumberOfBytesRead,NULL)) SendMessageList("ReadMyFile: Can't ReadFile");

else DrawFloor();

}

CloseHandle(hFile);

}

}

5. Написать функцию связывания карт навигационной система и помещения WhatPosition и рисования осей карты навигационной системы относительно осей карты помещения.

Всего существуют восемь вариантов взаимного расположения осей карты навигационной системы и карты помещения, если разместить маяки, как на рис. 4.1, и производить связывание, как в п. 4.2. На рис. 4.10 в качестве примера приведено взаимное расположение осей под номерами 1 и 2.

Объявить глобальные переменные:

static UINT8 position; static POINT X0_YN ; static POINT XN_YN ; static INT16 D;

static INT16 D_X0_YN;

В результате работы функции будет определен вариант взаимного расположения осей карт и запомнен в переменной position. Для этого используются координаты точек, запомненных в X0_Y0 и XM_YM, при выполнении процедуры построения карты помещения (п. 4.2).

28

X0_YN и XN_YN примут значения, являющиеся координатами точек на осях координат карты навигационной системы. Они используются только для рисования этих осей.

Впервой части функции вычисляются значения position и D,

аво второй осуществляется рисование расположения осей навигационной системы.

static void WhatPosition(void){ char szBuff[128];

double d,d1,a,Ax; POINT p;

//-------------------------Part 1-----------------------------------------

d=sqrt(pow((double)X0_Y0.x,2)+pow((double)X0_Y0.y,2)); if(d==0)

Ax=0; else

Ax=asin((double)abs(X0_Y0.y)/d); if(X0Y0_OFFSET){

if(d==0)

a=0;

else

a=asin((double)X0Y0_OFFSET/d);

Ax=Ax+a;

D=round(d*cos(a));

d=d*cos(a);

} else

D=round(d);

position=0;

if (X0_Y0.x<0&&X0_Y0.y>0&&XM_YM.x>0&&XM_YM.y>0) position=1;

else if(X0_Y0.x<0&&X0_Y0.y>0&&XM_YM.x<0&&XM_YM.y<0) position=8;

if (X0_Y0.x>0&&X0_Y0.y>0&&XM_YM.x<0&&XM_YM.y>0) position=2;

else if(X0_Y0.x>0&&X0_Y0.y>0&&XM_YM.x>0&&XM_YM.y<0) position=6;

if (X0_Y0.x<0&&X0_Y0.y<0&&(XM_YM.x>0&&XM_YM.y<0|| XM_YM.x>0&&XM_YM.y>0))

position=3;

else if(X0_Y0.x<0&&X0_Y0.y<0&&XM_YM.x<0&&XM_YM.y>0) position=7;

if (X0_Y0.x>0&&X0_Y0.y<0&&XM_YM.x<0&&XM_YM.y<0) position=4;

else if(X0_Y0.x>0&&X0_Y0.y<0&&XM_YM.x>0&&XM_YM.y>0)

30