книги / Прототипирование сетевой системы управления. Разработка Windows-приложения удаленного контроллера прототипа робота-официанта на базе PROMOBOT V
.4.pdfposition=5;
if (X0_Y0.x>=0&&X0_Y0.y==0&&XM_YM.x>0&&XM_YM.y>0)
position=9; |
|
//------------------------------- |
Part 2------------------------------------------------ |
switch(position){ |
|
case 1: |
|
case 2: |
|
case 3: |
|
case 4: |
|
if(d) |
|
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