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

position=5;

if (X0_Y0.x>=0&&X0_Y0.y==0&&XM_YM.x>0&&XM_YM.y>0)

a=asin((double)abs(X0_Y0.x)/d);

break; case 5: case 6: case 7: case 8:

if(d)

a=asin((double)abs(X0_Y0.y)/d);

break; case 9:

a=Ax;

break;

}

hdc=GetDC(ghwndDlg);

if (position>0&&position<9){ d1=d/sin(M_PI/2-a); D_X0_YN=round(d1); X0_YN.y=round(d1*sin(a));

if(position<5) hPen=CreatePen(PS_SOLID,3,BLUE);

else

hPen=CreatePen(PS_SOLID,3,GREEN); SelectObject(hdc,hPen); MoveToEx(hdc,D,0,NULL);

ineTo(hdc,0,X0_YN.y);

hPen=GetStockObject(NULL_PEN); SelectObject(hdc,hPen);

AngleArc(hdc, D,0, 600, 0, 270+(a*180)/M_PI); MoveToEx(hdc,D,0,&p);

XN_YN=p; MoveToEx(hdc,p.x,p.y,NULL);

if(position<5)

31

hPen=CreatePen(PS_SOLID,3,GREEN);

else

hPen=CreatePen(PS_SOLID,3,BLUE); SelectObject(hdc,hPen);

LineTo(hdc,D,0);

switch(position){ case 1: case 2: case 5: case 6:

hPen=GetStockObject(NULL_PEN); SelectObject(hdc,hPen); MoveToEx(hdc,0,X0_YN.y,NULL); hPen=CreatePen(PS_SOLID,10,BLACK); SelectObject(hdc,hPen); LineTo(hdc,0,X0_YN.y);

break;

default:

break;

}

switch(position){ case 1: case 3: case 5: case 7:

hPen=GetStockObject(NULL_PEN); SelectObject(hdc,hPen); MoveToEx(hdc,p.x,p.y,NULL); hPen=CreatePen(PS_SOLID,10,BLACK); SelectObject(hdc,hPen); LineTo(hdc,p.x,p.y);

break;

default:

break;

}

}else if (position==9){ hPen=CreatePen(PS_SOLID,3,GREEN); SelectObject(hdc,hPen); MoveToEx(hdc,D,0,NULL); LineTo(hdc,D+400,0); MoveToEx(hdc,D,0,NULL); hPen=CreatePen(PS_SOLID,3,BLUE); SelectObject(hdc,hPen); LineTo(hdc,D,400);

32

hPen=CreatePen(PS_SOLID,10,BLACK); SelectObject(hdc,hPen); LineTo(hdc,D,400);

hPen=GetStockObject(NULL_PEN); SelectObject(hdc,hPen); MoveToEx(hdc,D+400,0,NULL);

hPen=CreatePen(PS_SOLID,10,BLACK); SelectObject(hdc,hPen);

LineTo(hdc,D+400,0);

}else{ D_X0_YN=1; X0_YN.y=1;

}

ReleaseDC(ghwndDlg, hdc); DeleteObject(hPen);

sprintf(szBuff, "position=%d, D=%d, Dy=%d, X0YN.y=%d, a=%2.1f",position,D,D_X0_YN, X0_YN.y,round((Ax*180)/M_PI)); SendMessageList(szBuff);

}

6. Написать код, запускающий последовательность действий для вывода в диалоговое окно карты помещения из предложенного списка файлов при нажатии кнопки с идентификатором READ_FILE. Использовать функцию GetOpenFileName API Win32, выводящую для выбора список файлов. Объявить глобальную переменную:

static OPENFILENAME ofn;

Удаляется изображение прежней карты путем заполнения ее контура и самого контура цветом фона. GetOpenFileName открывает список файлов. Имя выбранного файла выводится в элемент Editbox с идентификатором IDEDITCMD и передается в функцию ReadMyFile. Вызов функции WhatPosition готовит всю необходимую информацию для преобразования координат.

case READ_FILE: hdc=GetDC(ghwndDlg);

hPen=CreatePen(PS_SOLID,2,FON_COLOR); SelectObject(hdc,hPen); hBrush=CreateSolidBrush(FON_COLOR); SelectObject(hdc,hBrush);

33

Polygon(hdc,&FloorPlan.Poligons[0][0],FloorPlan.PoligonNodesNum[0]); ReleaseDC(ghwndDlg, hdc);

DeleteObject(hPen);

DeleteObject(hBrush);

memset((void *)&ofn, 0, sizeof(OPENFILENAME)); ofn.lStructSize=sizeof(OPENFILENAME); ofn.hwndOwner=hwndDlg;

szBuff[0]=0;

ofn.lpstrFile=(LPTSTR)szBuff;

ofn.nMaxFile=128;

ofn.Flags=0;

if (FALSE==GetOpenFileName(&ofn )){ SendMessageList("LOAD_PROG: GetOpenFileName");

}else{

long int file_size;

SetDlgItemText(hwndDlg, IDEDITCMD, szBuff); ReadMyFile(szBuff,&file_size);

sprintf(szBuff, "file size=%d", file_size); SendDlgItemMessage(hwndDlg,IDLISTMES, LB_INSERTSTRING, 0,(LPARAM)szBuff);

X0_Y0=FloorPlan.X0_Y0; X0Y0_OFFSET=FloorPlan.X0Y0_OFFSET; XM_YM=FloorPlan.XM_YM; XM_YM=FloorPlan.XM_YM; WhatPosition();

}

return TRUE;

7. Написать функцию CoordinateTransform, которая возвращает координаты карты помещения, полученные из координат карты навигационной системы.

static POINT CoordinateTransform(POINT p){ POINT room_p;

double d0,d00,d; d00=pow((double)p.x,2)+pow((double)p.y,2);

d0=pow((double)(p.x-X0_Y0.x),2)+pow((double)(p.y-X0_Y0.y),2); d=pow((double)D,2);

room_p.x=abs(round((d0-d00+d)/(2*D))); room_p.y=abs(round(sqrt(d0-pow((double)room_p.x,2)))); return room_p;

}

34

8. Написать функции DelSpot и DrawSpot для удаления и рисования точки местоположения мобильного маяка на карте помещения.

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

static POINT OldPoint;

Объявить константу:

#define TRACK_CNT 50

Объявить глобальный массив точек и проинициализировать его первые 10 элементов:

Static POINT Track[TRACK_CNT]= {{0,0},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0},{0,0}};

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

static UINT16 TrackCnt=10; static UINT16 TrackCntDel=0;

Массив Track сохраняет последние TRACK_CNT полученных координат. Счетчик TrackCnt определяет позицию для записи в Track координат точки, а TrackCntDel – позицию в Track для удаления координат точки. Это позволит при перемещении мобильного маяка видеть на карте несколько его последних положений.

static void DelSpot(POINT p){ hdc=GetDC(ghwndDlg); hPen=CreatePen(PS_SOLID,5,FON_COLOR); SelectObject(hdc,hPen); MoveToEx(hdc,p.x,p.y,NULL); LineTo(hdc,p.x,p.y);

DeleteObject(hPen); ReleaseDC(ghwndDlg, hdc);

}

static void DrawSpot(POINT p,COLORREF color){ if((OldPoint.x==p.x)&&(OldPoint.y==p.y))

35

return;

OldPoint=p;

hdc=GetDC(ghwndDlg); hPen=CreatePen(PS_SOLID,5,color); SelectObject(hdc,hPen); MoveToEx(hdc,p.x,p.y,NULL); LineTo(hdc,p.x,p.y); DeleteObject(hPen); ReleaseDC(ghwndDlg, hdc); DelSpot(Track[TrackCntDel]); TrackCnt++;

if (TrackCnt>=TRACK_CNT) TrackCnt=0; Track[TrackCnt]=p; TrackCntDel++;

if (TrackCntDel>=TRACK_CNT) TrackCntDel=0;

}

9. Дополнить функцию DataProcessingGPS обработки ответа dashboard-сервера вызовом функции DrawSpot для отображения синей точкой текущего положения мобильного маяка на карте помещения.

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); p.x=X;

p.y=Y;

DrawSpot(CoordinateTransform(p),BLUE);

}

36

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

}

4.5. Задание маршрута движения

Маршрут движения задает последовательность координат точек карты помещения, соединяемых отрезками прямых.

1. Объявить константу, ограничивающую число точек маршрута:

#define PATH_POINTS_MAX 8

Объявить глобальную массив точек маршрута:

static POINT Path[PATH_POINTS_MAX];

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

static UINT16 PathLenth;

Прокладка маршрута выполнятся мышью: а) наведение курсора мыши в нужную точку;

б) нажатие правой кнопкой мыши добавляет очередную точку в массив Path (если отрезок прямой до предыдущей точки не проходит сквозь препятствие), выполняется рисование маршрута зеленым цветом;

в) нажатие левой кнопки мыши удаляет маршрут.

2. Написать функцию IsPointInPolygon, возвращающую значение 1, если указанная точка находится внутри указанного полигона

static UINT8 IsPointInPolygon(UINT16 X, UINT16 Y,

POINT *Poligon, int PolygonLength){

int i,j;

UINT8 isInside;

if (PolygonLength < 3) return 1;

isInside = 0;

for (i = 0, j = PolygonLength - 1; i < PolygonLength; j = i++) { if (((Poligon[i].y > Y) != (Poligon[j].y > Y)) &&

37

((double)X < ((double)Poligon[j].x - (double)Poligon[i].x) * ((double)Y -(double)Poligon[i].y) /

((double)Poligon[j].y - (double)Poligon[i].y) + (double)Poligon[i].x)) isInside ^= 1;

}

return isInside;

}

3. Написать функцию DrawPath рисования маршрута. Рисуются отрезки линий между соседними точками из массива Path.

static void DrawPath(void){ int i;

if(PathLenth){

hdc=GetDC(ghwndDlg); hPen=CreatePen(PS_SOLID,3,GREEN); SelectObject(hdc,hPen); MoveToEx(hdc,Path[0].x,Path[0].y,NULL); for (i=1;i<PathLenth;i++) LineTo(hdc,Path[i].x,Path[i].y); ReleaseDC(ghwndDlg, hdc); DeleteObject(hPen);

}

}

4. Объявить глобальную переменную:

static UINT8 InPolygon;

Для обнаружения пересечения с препятствием используется функция LineDDA API Win32, перебирающая все точки, лежащие на заданном отрезке.

Для обработки этих точек написать функцию обратного вызова LineDDAProc, проверяющую, попадает ли точка отрезка внутрь какого-либо полигона карты.

static void LineDDAProc(int x, int y, LPARAM Arg3)

{

for (int i=1;i<FloorPlan.PoligonNum;i++)

InPolygon |=IsPointInPolygon(x, y, &FloorPlan.Poligons[i][0], FloorPlan.PoligonNodesNum[i]);

}

38

5. В MainDlgProc объявить локальную переменную:

POINT Point;

Написать код обработки сообщения WM_RBUTTONUP, приходящего в MainDlgProc при нажатии правой кнопки мыши. В Point запоминаются координаты курсора мыши (используются функции GetCursorPos и ScreenToClient API Win32).

case WM_RBUTTONUP: GetCursorPos(&Point); ScreenToClient(hwndDlg,&Point); InPolygon=0;

if(PathLenth) LineDDA(Path[PathLenth-1].x, Path[PathLenth-1].y,Point.x,Point.y, LineDDAProc,(LPARAM )InPolygon); if (InPolygon==0){ Path[PathLenth]=Point;

if(PathLenth<(PATH_POINTS_MAX-1)){ PathLenth++;

hdc=GetDC(ghwndDlg); hPen=CreatePen(PS_SOLID,5,RED); SelectObject(hdc,hPen); MoveToEx(hdc,Point.x,Point.y,NULL); LineTo(hdc,Point.x,Point.y); ReleaseDC(ghwndDlg, hdc); DeleteObject(hPen);

DrawPath();

}

}

return TRUE;

6. Написать функцию DeletePath удаления маршрута:

static void DeletePath(void){ int i;

if(PathLenth){

hdc=GetDC(ghwndDlg); hPen=CreatePen(PS_SOLID,5,FON_COLOR); SelectObject(hdc,hPen); //FaceColor MoveToEx(hdc,Path[0].x,Path[0].y,NULL); for (i=1;i<PathLenth;i++)

LineTo(hdc,Path[i].x,Path[i].y);

39

ReleaseDC(ghwndDlg, hdc); DeleteObject(hPen);

}

}

Написать код обработки сообщения WM_LBUTTONUP, приходящего в MainDlgProc при нажатии левой кнопки мыши для удаления маршрута.

case WM_LBUTTONUP: DeletePath(); PathLenth=0; return TRUE;

4.6. Движение по маршруту

Управление движением по маршруту основано на трех действиях. Во-первых, если робот движется в направлении цели (очередная точка маршрута), то расстояние должно постоянно уменьшаться. Как только расстояние начинает увеличиваться, необходимо производить коррекцию ориентации робота.

Во-вторых, для ориентации робота в пространстве при отсутствии компаса необходимо мобильный маяк установить на роботе определенным образом: спереди со смещением относительно оси вращения, например, как показано на рис. 4.11.

На рис. 4.12 показан принцип ориентации робота по направлению к цели: вращение вокруг своей оси до тех пор, пока не будет обнаружено минимальное расстояние между мобильным маяком и целью.

В-третьих, во время движения могут быть обнаружены неожиданные препятствия, их необходимо объезжать и продолжать движение к цели.

Управление движением по маршруту необходимо реализовать в виде потока, запускаемого из диалогового приложения по нажатии кнопки. При обнаружении препятствий поток управления движением по маршруту должен приостанавливаться и запускаться поток, реализующий алгоритм объезда.

40