1、深圳帀中加h鑫智能技術有限公司 www jiqiren. com www. kongzhi com www zhinengdianqi com通訊和底層決策文檔根據robocup系統的四個子系統的分類，串口通訊的任務是將決策系統確定的每個機器人的輪速發送出 去.考慮到sendcommand()是每個機器人的在basicact()中調用的函數，所以把通訊的相關函數放在player 類中，m_hcomm存在于程序運行的始終，并與實例無關，設為靜態變量，initcomm()襦操作m_hcomm,故 設為靜態函數，其它與實例無關的函數也定義成靜態函數。/in player.hstatic handle

2、 m_hcomm;void sendcommand();static int initcomm(int comm_num, int baud);static bool writeabuffer(char * lpbuf, dword dwtowrite);static void sendcommandofall();static void sendsinglecommand(const unsigned char command);sendcommando為每個機器人單獨調用的函數，作用是將設置到該機器人(g_myteami)中去的速度 m_vl,m_vr通過串口發送出去。其他各函數的功能及有

3、關說明見函數中的注釋。/in playerepp/功能：初始化串口參數：comm串口號，1或2； baud:波特率返冋：若成功返冋1，失敗則返冋一 1int player:initcomm(int comm_num, int baud)cstring strcommnum;if(l = comm_num)strcommnum = "comi”；elsestrcommnum = "com2"m_hcomni = createfile( strcommnum,generic_read i generic.write,0,0,open_existingfile_flag

4、_overlapped,0)；if (m_hcomm = invalid_handle_value)/)(afxmessagebox(h串 口初始化火敗“)；return -1;/ error opening port; abort設置dcb結構深圳 11 j中加 口鑫智能技術右限公司 www. jiqiren. com www. kongzhi. com www. zhinengdianqi. coin dcb deb;fillmemory(&dcb, sizeof(dcb), 0);dcb.dcblength = sizeof(dcb);cstring str;cstring st

5、rl;strl.format("%d", baud);str = strl + ”,n,8,1"bool b = buildcommdcb(str, &dcb);將參數寫進 debif(!b)若失敗afxmessagebox(nbuildcommdcb failed!'1);return -1;)if(!setcommstate(m_hcomm, &deb)/根據 deb 設置串 口狀態，若失敗afxmessagebox(h 串口初始化失敗！ “)；return -1;return 1;功能：先設泄命令字command,再用sendsing

6、lecommand(command)將該命令字發送出去命令為1個字節：最高三位小車編號001j00為1 (g_myteam0)到4 (g_myteam3)號,000為5號第四位為左右輪標識：0 ：左，1 ：右輪第五位為速度方向：0：向詢；1 :向灰最低三位為速度大小：000-111從小到人八檔速度參數：無返回：無void player:sendcommand()將單個小車的速度發送出去unsigned char command;/一個字節unsigned cheir temp;/暫存 command先發左輪再發右輪for(unsigned char j=o;j<=l;j+)/j=o 為左

7、輪，j=l 為右輪設定小車編號if(this->get_num()=4)/為 5 號時發送 000temp=0;elsetemp=this->get_num()+1;深圳 11 j 中加 口鑫智 能技術有限 公司 www. jiqiren. com www. kongzhi. com www. zhinengdianqi. coin tempvv=5;/左移5位得到高三位，小車編號設定左右輪標識unsigned char charv;charv=(unsigned char)j«4;temp=temp i charv;/第四位左右輪設定速度方向unsigned char

8、mask;int t=l;if(j=o)(mask=(*this).get_vl()<0 ? (t«3):0);else /j=lmask=(*this).get_vr()<0 ? (t«3):0);temp=temp i mask;/第五位速度方向,速度向后為1,向前為0設定速度人小int m_floor=0;/0-7ifu=o)/分別設置速度，最低三位 m_floor=(int)floor(fabs(*this).get_vl()/(max_velocity/8)-0.5); if(m_floor>=8) m_floor=7;if(m_floor<

9、;0) m_floor=0; temp=temp i m_floor;else/j= 1 時 m_floor=(int)floor(fabs(*this).get_vr()/(max_velocity/8)-0.5); if(m_floor>=8) m_floor=7;if(m_floor<0) m_floor=0; temp=temp i m_floor;comm<ind=temp;發送命令sendsinglecommand(command);)深圳 11 j中加 口鑫智能技術右限公司 www. jiqiren. com www. kongzhi. com www. zhi

10、nengdianqi. coin 功能：將command命令字節發送出去為確保數據的準確接收，發送中增加一些字節frameheadl (0x55)用于建立連接framehead2 (oxff)為兀余字節，可去掉framehead3 (0x41)幀頭用于引導，表示下面為控制字/參數：command為命令字節返冋：無void player: :sendsinglecommand(unsigned cheir command)tchar framehead 1 ;/unicodetchar framehead2;tchar framehead3;framehead 1 =(tchar)0x55;fr

11、amehead2=(tchar)0xff;framehead3 = (tchar)0x41;writeabuffer(&framehcad 1, 1);建立連接writeabuffer(&frameheadl, 1);writeabuffer(&framehead2, 1);/冗余writeabuffer(&framehead2, 1);writeabuffer(&framchead3, 1);發幀頭writeabuffer(&fnimehead3, i);writeabuffer(tchar*)&command, 1);/命令字writ

12、eabuffer(&framehead2, 1);/jl 余writeabuffer(&fnimehead2, i);/功能：將所有機器人的速度同時發出參數：無參數：無void player:sendcommandofall()/通訊系統接口unsigned char command; unsigned char temp;for(unsigned char j=o;j<=l;j+)/j=o 為左輪，j=l 為右輪if(i=4)/為5號時發送000 temp=0;地址: 郵編: 電話: 傳真:深圳市振興路金茂禮都b棟28l5180310755-82810518828105

13、280755-82810538深圳 11 j中加 口鑫智能技術右限公司 www. jiqiren. com www. kongzhi. com www. zhinengdianqi. coin elsetemp=i+l;tempvv=5;左移5位得到高三位，小千編號unsigned char charv;charv=(unsigned char)j«4;temp=temp i charv;/第四位左右輪unsigned char mask;int t=l;ifo=0)mask=(g_myteami.get_vl()<0 ? (t«3):0);else /j=lmask

14、=(g_myteami.get_vr()<0 ? (t«3):0);temp=temp i mask;/第五位速度方向，速度向后為1,向前為0int m_floor=0;/0-7 ifo=o)/分別設置速度，最低三位 m_floor=(int)floor(fabs(g_myteami.get_vl()/(max_velocity/8)-0.5);if(m_floor>=8) m_floor=7;if(m_floor<0) m_floor=0;temp=temp i m_floor;else/j=l m_noor=(int)floor(fabs(g_myteami.g

15、et_vr()/(max_velocity/8)-0.5);if(m_floor>=8) m_floor=7;if(m_floor<0) m_floor=0; temp=temp i m_floor;comm<ind=temp;sendsinglecomniand(command);深圳 11 j中加 口鑫智能技術右限公司 www. jiqiren. com www. kongzhi. com www. zhinengdianqi. coin功能：把從ipbbuf開始的dwtowrite個字節寫入h_mcomm屮參數：lpbuf為開始地址，dwtowrite為需要寫的字節返冋

16、：若成功返冋true,失敗返冋falsebool player:writeabuffer(char * lpbuf, dword dwtowrite)overlapped oswrite = 0;dword dwwritten;bool fres;oswrite.hevent = createevent(null, true, false, null);/ 注意第二個參數if (oswrite.hevent = null)(/ error creating overlapped event handle.return false;/ issue write.if (!writefile(m_h

17、comm, lpbuf, dwtowrite, &dwwritten, &oswrite) /dwwrilten? 訐(getlasterror() != erroro.pending) /沒有調用/ writefile failed, but it isn't delayed. report error and abort.fres = false;afxmessagebox("fail 1");else/ write is pending.if (!getoverlappedresult(m_hcomm, &oswrite, &d

18、wwritten, true) fres = false;afxmessagebox(mfail2m);elsefres = true;)else/ writefile completed.fres = true;)深圳lij中力u 口鑫智能技術有限公司 www jiqirencom wuywkongzhicom www zhinengdianqi comclosehandle(oswrite.hevent);retui-n fres;底層決策可以從兒個基本的類談起：vector類：用于向量運算，數據元素x,y為橫縱處標。類中重載了-些運算符，提供的函數有：/in viector.hinlin

19、e double mod() return double(sqrt(x*x+y*y); 取模inline double mod2() return x*x + y*y; /square modvoid normalize() if (mod2() != 0) (*this) /= mod(); 單位化double dist(vector& pos) return (*this-pos).mod(); 到 pos 的距離double dist2(const vector &a) return (*this-a).mod2(); /square distdouble getangi

20、e();得到向量幅角vector rotate(double ang);旋轉一個角度/in vector.cpp功能：旋轉一個角度vector vector: :rotate(double ang)ang *= 0.01745329222;retum vector(cos(ang)*x sin(ang)*y,sin(ang)*x + cos(ang)*y);功能：得到向量幅角返冋：得到180到180z間的角double vector:getangle()double k;k = atan2(y, x);return 57.29578049*k;object類：為player的父類，場上物休為小

21、球和機器人小車，它們的屬性有尺寸大小和在場上的位置.類 中提供了獲得和設置這些屬性的方法。player類:player為object的派牛類，屬性有小車編號m_num,左右輪速m_vl和m_vr,小車朝向角 m.angle,均為私有變量，類中定義了獲得和設置這些屬性的方法。串口通訊的相關斷數也放到該類中，町 參考串口部分。小車的基本動作全部放到該類中，再此只討論以下兒個動作函數：功能：最底層動作，上層動作的實現最終依賴于它，其功能是根據參數設置小車左右輪速，設置好后 馬上將命令發送出去參數：r為轉彎半徑（只為正）blsrighthand為運動軌跡是否要符合右手定則，bisline為是否走直線,

22、v 為（平均）速度（可止可負）返冋：無void player:basicact（double r, bool blsrighthand, bool bisline, double v）深圳 11 j 中加 口鑫智 能技術有限 公司 www. jiqiren. com www. kongzhi. com www. zhinengdianqi. coin if(fabs(v) < 0.1)/要加絕對值v = 0;if(bisline)/不管r是多少，直線行走/r為無窮大set_vl(v);set_vr(v);elseif(bisrigh(hand)右手定則(即右手畫冏)，如果v>()向

23、左轉，如果vvo后左傳double vl, vr;vr = v;vl = (2*r-m_length)*vr/(2*r+m_length);/ 確保左右輪速協調set_vl(vl);set_vr(vr);elsedouble vl, vr;vl = v;vr = (2*r-m_length)*vl/(2*r+m_length);set_vl(vl);set_vr(vr);sendcommando;/將命令發送! h去bool player:turnto (vector desl)/返回值代表的并不是是否已經轉到double angle;/角度值 angle=(dest-(this->get_vpos().getangle()-(this->get_m_angle();/ 兩個角度差值 static depth=o;if (depth>4)/三次轉不到則放棄，并返冋falsekilltimer(cdebugdlg:m_pdebugdlg->m_hwnd,id_timer);basicact(o,false,false,0);depth=o;return false;深圳 11 j中加 口鑫智能技術右限公司 www. jiqiren. com www. kongzhi. com www. zhinengdianqi.