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

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

static float MarvelmindTargetDistance; static INT16 TargetX,TargetY;

static UINT8 EnMoveAlongTheTrack;

static HANDLE hMoveAlongTheTrack=NULL; static UINT8 EscapeTrackPiece=1;

static HANDLE hBypassThread=NULL; static UINT8 EnBypassThread=0;

static HANDLE hStartBypassOK; static HANDLE hStopBypassOK; static HANDLE heventEscape; static HANDLE TurnOK;

В разделе инициализации (WM_INITDIALOG) создать события:

TurnOK=CreateEvent(NULL,TRUE,FALSE,NULL); hStartBypassOK=CreateEvent(NULL,TRUE,FALSE,NULL ); hStopBypassOK=CreateEvent(NULL,TRUE,FALSE,NULL ); heventEscape=CreateEvent(NULL,TRUE,FALSE,NULL );

Переменная TurnOK сохраняет идентификатор объекта события, сигнализирующего о завершении разворота в сторону целевой точки.

Переменная hStartBypassOK сохраняет идентификатор объекта события, сигнализирующего об обнаружении препятствия.

Переменная hStopBypassOK сохраняет идентификатор объекта события, сигнализирующего об окончании объезда препятствия.

Переменная heventEscape сохраняет идентификатор объекта события, сигнализирующего об истечении времени, отведенного на выполнение того или иного действия.

2. Для удобства чтения программы переопределить имена датчиков приближения:

#define FRONT_LEFT s_8 #define FRONT_LEFT_LEFT s_9

#define FRONT_LEFT_CORNER s_10 #define FRONT_RIGHT s_6

42

#define FRONT_RIGHT_RIGHT s_5 #define FRONT_RIGHT_CORNER s_4

#define BACK_LEFT s_14 #define BACK_LEFT_LEFT s_13 #define BACK_RIGHT s_16

#define BACK_RIGHT_RIGHT s_1

Дополнить перечислимый тип state_t значениями

ROTATE_RIGHT,

ALONG_THE_WALL_LEFT,

ALONG_THE_WALL_RIGHT,

WHERE_THE_WALL_LEFT,

WHERE_THE_WALL_RIGHT,

BACK_AFTER_LEFT,

BACK_AFTER_RIGHT,

RESUME

3. Для более плавного переключения между направлениями движения в начало функции ForwardBot поместить операторы:

if (move!=ONWARD&&move!=STOP){ StopBot();

Sleep(500);

}

в начало функции BackBot поместить операторы:

if (move!=BACKWARD&&move!=STOP){ StopBot();

Sleep(500);

}

в начало функции LeftRotateBot поместить операторы:

if (move!=TURN_LEFT&&move!=STOP){ StopBot();

Sleep(500);

}

43

в начало функции RightRotateBot поместить операторы:

if (move!=TURN_RIGHT&&move!=STOP){ StopBot();

Sleep(500);

}

4. Написать функцию BypassThread потока, управляющего объездом препятствий. Сначала необходимо выяснить, может ли препятствие самоустраниться, – подождать, например, 2,5 с. Если препятствие не исчезает, осуществить поворот к нему левым или правым боком и движение вдоль препятствия до тех пор, пока не появится свободное место с той стороны, вдоль которой осуществляется движение.

static DWORD WINAPI BypassThread(LPVOID lpParam){ ResetEvent(hStopBypassOK);

SetEvent(hStartBypassOK);

EnBypassThread=1;

state=IMMOBILITY; SendMessageList("START----------BypassThread------------"); Sleep(GPS_POLLING_PERIOD*20);

if ( SeeTheWallOnward()==0){ state=RESUME; EnBypassThread=0; SendMessageList("state=RESUME");

}else if (s[FRONT_LEFT]< s[FRONT]|| s[FRONT_LEFT_LEFT]<s[FRONT_LEFT]){

RightRotateBot(); state=ROTATE_RIGHT;

SendMessageList("state=ROTATE_RIGHT");

}else{

LeftRotateBot(); state=ROTATE_LEFT;

SendMessageList("state=ROTATE_LEFT");

}

while(EnBypassThread){ Sleep(GPS_POLLING_PERIOD*10); switch (state){

case ROTATE_LEFT:

if (s[FRONT]>RANGE_MAX&& s[FRONT_RIGHT_RIGHT]>RANGE_MAX&& s[FRONT_RIGHT]>RANGE_MAX||

44

s[BACK_RIGHT_RIGHT]<=MIN_DIST_TO_WALL){ state=ALONG_THE_WALL_RIGHT; StopBot();

Sleep(GPS_POLLING_PERIOD); ForwardBot();

SendMessageList("state=ALONG_THE_WALL_RIGHT");

}

break;

case ROTATE_RIGHT:

if (s[FRONT]>RANGE_MAX&& s[FRONT_LEFT_LEFT]>RANGE_MAX && s[FRONT_LEFT]>RANGE_MAX|| s[BACK_LEFT_LEFT]<= MIN_DIST_TO_WALL){

state=ALONG_THE_WALL_LEFT; StopBot(); Sleep(GPS_POLLING_PERIOD); ForwardBot();

SendMessageList("state=ALONG_THE_WALL_LEFT");

}

break;

case WHERE_THE_WALL_LEFT: if (SeeTheWallOnward()){

StopBot(); Sleep(GPS_POLLING_PERIOD); RightRotateBot(); state=ROTATE_RIGHT; SetTimer(ghwndDlg, 8,80000,NULL);

SendMessageList("state=ROTATE_RIGHT"); }else if (s[SIDE_FRONT_LEFT]<RANGE_MAX_MAX){

state=ALONG_THE_WALL_LEFT; SendMessageList("state=ALONG_THE_WALL_LEFT");

}

break;

case ALONG_THE_WALL_LEFT: if(SeeTheWallOnward()){

StopBot(); Sleep(GPS_POLLING_PERIOD); ObstacleAvoidance();

}else if(NotSeeTheWallLeft()){ Sleep(GPS_POLLING_PERIOD*2); if(NotSeeTheWallLeft()){

StopBot(); Sleep(GPS_POLLING_PERIOD); state=IMMOBILITY;

45

}

}

break;

case WHERE_THE_WALL_RIGHT: if (SeeTheWallOnward()){

StopBot(); Sleep(GPS_POLLING_PERIOD); LeftRotateBot(); state=ROTATE_LEFT; SetTimer(ghwndDlg, 8,80000,NULL);

SendMessageList("state=ROTATE_LEFT");

}else if (s[SIDE_FRONT_RIGHT]<RANGE_MAX_MAX){ state=ALONG_THE_WALL_RIGHT; SendMessageList("state=ALONG_THE_WALL_RIGHT");

}

break;

case ALONG_THE_WALL_RIGHT: if (SeeTheWallOnward()){

StopBot(); Sleep(GPS_POLLING_PERIOD); ObstacleAvoidance();

}else if(NotSeeTheWallRight()){ Sleep(GPS_POLLING_PERIOD*2); if(NotSeeTheWallRight()){

StopBot(); Sleep(GPS_POLLING_PERIOD); state=IMMOBILITY;

}

}

break;

case BACK_AFTER_LEFT:

if SeeTheWallOnwardForBack()==0){ StopBot(); Sleep(GPS_POLLING_PERIOD); LeftRotateBot(); state=ROTATE_LEFT; Sleep(GPS_POLLING_PERIOD*20);

SendMessageList("state=ROTATE_LEFT");

}

break;

case BACK_AFTER_RIGHT:

if SeeTheWallOnwardForBack()==0){ StopBot(); Sleep(GPS_POLLING_PERIOD); RightRotateBot();

46

state=ROTATE_RIGHT; Sleep(GPS_POLLING_PERIOD*20); SendMessageList("state=ROTATE_RIGHT");

}

break;

}

if (move==STOP) break;

}

if(state==RESUME)

ForwardBot(); KillTimer(ghwndDlg, 8); EnBypassThread=0; SetEvent(hStopBypassOK); ResetEvent(hStartBypassOK); CloseHandle(hBypassThread); hBypassThread=NULL;

SendMessageList("STOP ----------BypassThread------------"); state=IMMOBILITY;

return 0;

}

5. Написать функцию DistanceToTheOrigin, вычисляющую расстояние между двумя точками, представленными их координатами:

static float DistanceToTheOrigin(INT16 x,INT16 y, INT16 x_target, INT16 y_target){

return sqrtf(powf((float) (x-x_target),2)+powf((float)(y-y_target),2));

}

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

static float MarvelmindTargetDistance; static INT16 TargetX,TargetY;

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

40 см: EscapeTrackPiece=1; .

47

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){

d=DistanceToTheOrigin(xx,yy,X,Y);

if (((xx!=X)||(yy!=Y))&&(d<50)&&(X!=0x7fff)){

MarvelmindTargetDistance=DistanceToTheOrigin(xx,yy,

TargetX,TargetY);

sprintf(szBuffer,"d=%4.0f ",MarvelmindTargetDistance); SetDlgItemText(ghwndDlg,IDDISTBEACON,szBuffer); if ((INT16)MarvelmindTargetDistance<=40 &&

EscapeTrackPiece==0 && EnMoveAlongTheTrack){ EnBypassThread=0;

EscapeTrackPiece=1;

SetEvent(heventEscape);

KillTimer(ghwndDlg,8);

StopBot();

}

}

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);

}

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

}

7. Написать функцию, преобразующую Room Coordinate Transform, выполняющую преобразование координат карты помещения в координаты Indoor GPS:

48

static POINT RoomCoordinateTransform(POINT p){ POINT mm_p;

char Text[64]; double d0,d00,d;

POINT Poligon[4],Poligon1[4],Poligon2[4]; if(position==0){

mm_p.x=p.x; mm_p.y=p.y;

}else{

d00=pow((double)(D-p.x),2)+pow((double)p.y,2); d0=pow((double)p.x,2)+pow((double)(p.y-X0_YN.y),2); d=pow((double)D_X0_YN,2); Poligon[0].x=Poligon[0].y=Poligon[2].x=Poligon[1].y=0; Poligon[1].x=D;

Poligon[2].y=X0_YN.y; Poligon1[0].x=D; Poligon1[0].y=0; Poligon1[1].x=XN_YN.x; Poligon1[1].y=XN_YN.y; if (XN_YN.y>=X0_YN.y){

Poligon1[2].x=0; Poligon1[2].y=XN_YN.y;

}else{

Poligon1[2].x=650;

Poligon1[2].y=650;

}

Poligon1[3].x=0; Poligon1[3].y=X0_YN.y; Poligon2[0].x=D; Poligon2[0].y=0; Poligon2[1].x=XN_YN.x; Poligon2[1].y=XN_YN.y; Poligon2[2].x=XN_YN.x; Poligon2[2].y=0; switch(position){

case 1: mm_p.y=D_X0_YN-abs(round((d0-d00+d)/(2*D_X0_YN))); mm_p.x=abs(round(sqrt(d00-pow((double)mm_p.y,2))));

if (IsPointInPolygon(p.x, p.y, Poligon,3)) mm_p.x=-mm_p.x;

break; case 2:

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

if (IsPointInPolygon(p.x, p.y, Poligon,3)==0)

49

mm_p.x=-mm_p.x; break;

case 3: mm_p.y=D_X0_YN-abs(round((d0-d00+d)/(2*D_X0_YN))); mm_p.x=abs(round(sqrt(d00-pow((double)mm_p.y,2))));

if (IsPointInPolygon(p.x, p.y, Poligon,3)==1){ mm_p.x=-abs(mm_p.x); mm_p.y=-abs(mm_p.y);

}else if(IsPointInPolygon(p.x, p.y, Poligon1,4)==1){ mm_p.x=abs(mm_p.x); mm_p.y=-abs(mm_p.y);

}else if(IsPointInPolygon(p.x, p.y, Poligon2,3)==1){ mm_p.x=abs(mm_p.x); mm_p.y=abs(mm_p.y);

}

break; case 4:

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

if (IsPointInPolygon(p.x, p.y, Poligon,3)==1){ mm_p.x=abs(mm_p.x); mm_p.y=-abs(mm_p.y);

}else if(IsPointInPolygon(p.x, p.y, Poligon1,4)==1){ mm_p.x=-abs(mm_p.x); mm_p.y=-abs(mm_p.y);

}else if(IsPointInPolygon(p.x, p.y, Poligon2,3)==1){ mm_p.x=-abs(mm_p.x); mm_p.y=abs(mm_p.y);

}

break;

case 5: mm_p.x=D_X0_YN-abs(round((d0-d00+d)/(2*D_X0_YN))); mm_p.y=abs(round(sqrt(d00-pow((double)mm_p.x,2))));

if (IsPointInPolygon(p.x, p.y, Poligon,3)==1){ mm_p.x=abs(mm_p.x); mm_p.y=-abs(mm_p.y);

}else if(IsPointInPolygon(p.x, p.y, Poligon1,4)==1){ mm_p.x=abs(mm_p.x); mm_p.y=abs(mm_p.y);

}else if(IsPointInPolygon(p.x, p.y, Poligon2,3)==1){ mm_p.x=-abs(mm_p.x); mm_p.y=abs(mm_p.y);

}

break;

50