六自由度機(jī)械手用戶手冊(cè)

目錄TOC\o"1-2"\h\u6402第一章準(zhǔn)備安裝 SG5/6機(jī)械手 21006所需工具 224179SG5/6基本配置機(jī)械手包括下列元器件： 218168第二章安裝前準(zhǔn)備 41887注意： 429107準(zhǔn)備伺服電機(jī) 49164第三章組裝底座 57988安裝電機(jī)到底座盒 52324第四章組裝上臂 1517307第五章組裝前臂 2517986第六章組裝手爪 366025第七章裝手爪到前臂上 632089第八章彈簧安裝和連接電機(jī)電纜 6530810第九章教學(xué)板和電機(jī)控制器的連接 7021319第十章使用和操作機(jī)械手時(shí)必須注意的事項(xiàng)： 7210999第十一章機(jī)械手例程 7427060例程1電機(jī)調(diào)試 747809例程2手臂的基本動(dòng)作 779655例程3抓取和放置動(dòng)作 8829999螺旋槳舵機(jī)控制器USB（＃28830） 985546特點(diǎn) 983329應(yīng)用創(chuàng)意 9819542主要規(guī)格 9820288在連接你的螺旋槳伺服控制器USB之前 9812533連接和測(cè)試 981258USB連接 9813433TTL串行連接 998050通信協(xié)議 10015815命令集 1008313位置命令——設(shè)置位置伺服通道 10027907VER？-確定固件版本號(hào) 10124690SBR-設(shè)置波特率（2400或38.4K波特） 1025677PSS-設(shè)置軟件端口(范圍0-15或16-31) 10313138PSD-伺服禁用 1035523PSE-伺服啟用 1049136EDD-啟動(dòng)伺服模式 10429927默認(rèn)位置命令-設(shè)置默認(rèn)位置的伺服頻道 10510清除-清除EEPROM上數(shù)據(jù) 10619964硬件復(fù)位按鈕 1064870編程跳線 107119模塊尺寸 10814838技術(shù)支持 108第一章準(zhǔn)備安裝 SG5/6機(jī)械手所需工具下面是安裝SG5/6時(shí)所需要的工具：螺絲刀鉆孔器1/8英寸的鉆頭小型號(hào)的活動(dòng)扳手或套筒扳手剪鉗少量潤(rùn)滑油或類似物品完全配置機(jī)械手物品清單SG5/6基本配置機(jī)械手包括下列元器件：電子電器類\軟件：SG5：3個(gè)HiTecHS-475HB伺服電機(jī)，1個(gè)HS-805BB伺服電機(jī)，1個(gè)HS-645MG伺服電機(jī)SG6：3個(gè)HiTecHS-475HB伺服電機(jī)，1個(gè)HS-805BB伺服電機(jī)，1個(gè)HS-225MG伺服電機(jī)，1個(gè)HS-645伺服電機(jī)電機(jī)延長(zhǎng)線：12英寸長(zhǎng)2條，6英寸長(zhǎng)2條1個(gè)SPST開關(guān)1個(gè)6V-4A電源（只在完全配置中提供）鋁質(zhì)部件：1個(gè)電路板固定支架1個(gè)底座底座上轉(zhuǎn)盤下托板底座上轉(zhuǎn)盤上托板3個(gè)電機(jī)支4個(gè)手臂部件2個(gè)左手爪部件2個(gè)右手爪部件4個(gè)扁平連接件1齒輪支架1個(gè)電機(jī)齒輪傳動(dòng)支架1個(gè)前臂支架1個(gè)上臂支架螺母、螺釘、墊圈和取間隔裝置4個(gè)#2螺母4個(gè)#2彈墊4顆#2-5/16英寸螺釘35個(gè)#4平墊13顆#4-1/2英寸螺釘12顆#4-3/8英寸螺釘26顆#4-1/4英寸螺釘4顆#4-1英寸螺釘1顆#4-5/8英寸螺釘3顆#4-5/16英寸螺釘19顆#4螺母23顆#4鎖緊螺母19個(gè)#4彈墊14個(gè)#4-1/4英寸取間隔尼龍支柱4個(gè)#4-5/16英寸取間隔尼龍支柱8個(gè)#4-3/16英寸取間隔尼龍支柱14個(gè)#6-3/8英寸螺釘1個(gè)#6-1/2英寸螺釘8個(gè)#6鎖緊螺母8個(gè)#6螺母8個(gè)#6彈墊1個(gè)#6平墊6個(gè)1/4英寸SAE平墊4個(gè)球形支撐件其他1本SG5/6機(jī)械手手冊(cè)10條扎帶1根長(zhǎng)彈簧1根短彈簧2個(gè)齒輪4個(gè)橡膠墊2個(gè)夾子第二章安裝前準(zhǔn)備注意：在一個(gè)整齊、干凈有較大空間的環(huán)境下開展組裝工作整理和恰當(dāng)?shù)財(cái)[放你的各類不規(guī)格的螺釘、螺母等配件，這樣有利于你在裝配時(shí)正確的使用特定大小的螺釘、螺母、墊圈等，也可以很方便地取用它們。從容不迫！SG5/6機(jī)械手是一個(gè)精密的產(chǎn)品，它要求所有部件都按這個(gè)手冊(cè)中講的順序安裝。組裝好機(jī)械手的平均時(shí)間在3到6小時(shí)。在安裝的過程中請(qǐng)經(jīng)常參考本安裝手冊(cè)中提供的參考圖片。在開始組裝任何部件之前，請(qǐng)仔細(xì)參考和研究本手冊(cè)中提供的安裝圖片特別是那些特寫的圖片。必須一直注意螺釘和其他鋁材的方位和方向以及裝在機(jī)械手的那側(cè)。因?yàn)椴煌难b法可導(dǎo)致完全不同的結(jié)果。準(zhǔn)備伺服電機(jī)去掉從它們的保護(hù)包裝中去掉鋁件然后把它們放回在你工作空間里各自的類別中去。把六自由度機(jī)械手的六個(gè)伺服電機(jī)（五自由度為5個(gè)）上圓形的輸出臂從電機(jī)輸出周上取下并把它們放好。注：其中的一個(gè)HS-475HB電機(jī)上沒有圓形的輸出臂，去掉電機(jī)上所有的塑料突起（圖1所示），這樣做有利于把伺服電機(jī)更牢固地固定在電機(jī)固定支架上。這些電機(jī)在后面的步驟中將被陸續(xù)安裝。型號(hào)為HS805BB的較大一些的電機(jī)在剪取塑料突起后，可能還需要銼刀修整一下，因?yàn)樗乃芰贤黄鸨绕渌麕讉€(gè)電機(jī)的要厚。圖1第三章組裝底座安裝電機(jī)到底座盒圖21．用2顆#6-3/8英寸螺釘和彈墊，把一個(gè)HS475HB電機(jī)固定到底座盒上如圖2所示。注意讓伺服電機(jī)的中線和安裝孔的中線重合如圖3所示。將電機(jī)的電纜從底座盒側(cè)面中的狹引出。圖32．用2顆#4-3/8英寸的螺釘，平墊，彈墊和螺母，如圖4所示將電路板固定支架安裝到底座盒上。在擰緊#4螺釘前，把4個(gè)橡膠裝到電路板固定支架的底端，在必要時(shí)調(diào)整支架的高度于工作平面齊平（圖6），然后擰緊#4-3/8英寸的螺釘。圖4圖53．用4顆5/16英寸#4取間尼龍支柱、平墊，把4個(gè)支撐小件安裝到底座盒上如圖6所示。在裝支撐小件之前，#4平墊可以放在5/16英寸長(zhǎng)取間尼龍支的頂端。用手?jǐn)Q緊4個(gè)支撐小件，不要用鉗子或其他工具來擰它們。4．在每個(gè)支撐小件上涂適量的潤(rùn)滑油脂如圖7所示，之后暫時(shí)把該組件放到一邊。圖6圖7裝配完成后的底座盒5．用一個(gè)1/8英寸大小的鉆頭，把圖8中所示的孔擴(kuò)大。圖86．用4顆3/8英寸螺釘、鎖緊螺母把圓形的電機(jī)輸出臂安裝到底座轉(zhuǎn)盤下托板如圖9所示。圓形的電機(jī)輸出臂必須安裝在托板上有四個(gè)圓柱的一側(cè)（見圖10）。7．把托板裝到機(jī)械手底座上，順時(shí)鐘和逆時(shí)鐘轉(zhuǎn)動(dòng)托板以便讓在上面步驟中涂在支撐小件上的油脂均勻涂在下托板上，如圖10所示。圖108．卸下托板，用手指將油脂均勻涂到在上面步驟中轉(zhuǎn)動(dòng)托盤形成的油脂圓圈上，如圖11所示。重新涂少量油脂到支撐小件上。再把托盤裝回底座盒，這個(gè)時(shí)候還能用電機(jī)螺釘擰緊底座。圖119.臨時(shí)把下轉(zhuǎn)動(dòng)托盤裝到底座盒上，旋轉(zhuǎn)并調(diào)整轉(zhuǎn)盤使它能夠從中心狹槽向左和向右各轉(zhuǎn)90度角，如圖12所示。圖1210．當(dāng)轉(zhuǎn)動(dòng)托盤的位置調(diào)整正確后，用伺服電機(jī)上的螺釘把轉(zhuǎn)盤固定好。注意不要用力過猛，以免擰壞電機(jī)的輸出軸。擰緊螺釘?shù)睫D(zhuǎn)盤能夠自由地轉(zhuǎn)動(dòng)又不會(huì)因?yàn)樘啥罐D(zhuǎn)盤接觸不到底座盒上4個(gè)支撐小件。11．用8顆#4-1/2英寸螺釘、螺母和3/16英寸的取間尼龍支柱，把Parallax控制電路板、微型控制器、Parallax電機(jī)控制器、1個(gè)SPST開關(guān)（可選）、電源插頭（當(dāng)您選擇購(gòu)買了6V、4安培電源時(shí)——圖13b）安裝到電路板固定支架上。控制電路板和電機(jī)控制器的朝向應(yīng)如圖13所示。電路板固定支架適合所有型號(hào)的Parallax控制電路板和電機(jī)控制器。如果你自己的電路板和支架不配套只需用工具擴(kuò)一下孔就行。注意：為了您的方便我們提供了一個(gè)電源開關(guān)，電源可以直接接到電機(jī)控制器上也可以通過開關(guān)再到電機(jī)控制器。開關(guān)可用可不用，不影響機(jī)械手的正常操作。圖13圖13b第四章組裝上臂12．用3顆#4螺釘，彈墊和螺母，把電機(jī)安裝支架固定到上轉(zhuǎn)動(dòng)托盤上如圖14所示。擰緊螺釘前必須注意讓托盤上的孔和電機(jī)固定支架上的孔重合。圖1413．用1/8英寸的鉆頭，把如下圖15所示電機(jī)輸出臂上的孔擴(kuò)大，有兩個(gè)圓形電機(jī)輸出臂需要擴(kuò)孔，型號(hào)為645和475的各一個(gè)，把輸出臂裝到電機(jī)輸出軸上鉆會(huì)更容易鉆一些。圖1514．用2顆1/4英寸的螺釘、彈墊把電機(jī)圓形的輸出臂裝到機(jī)械手上臂上如圖16和圖17所示。圖16圖1715．用1顆#6-9/16英的螺釘、3個(gè)1/4英寸的SAE平墊和1個(gè)鎖緊螺母把右上臂安裝到電機(jī)固定支架上的狹槽上如圖18和圖19所示。適當(dāng)擰緊#6螺釘，讓上臂和支架之間沒有空隙但又不太緊而讓臂的運(yùn)動(dòng)受阻。理想的狀態(tài)是臂能豎起而不會(huì)倒下，圓形輸出臂朝內(nèi)側(cè)。圖18注意正確安排2個(gè)1/4英寸SAE平墊的次序，否則整個(gè)組件會(huì)變松動(dòng)。圖1916．用1/8英寸的鉆頭，把輸出臂上任意兩個(gè)相對(duì)的孔擴(kuò)大如圖20所示。孔鉆好后把輸出臂從電機(jī)上取下，重新把電機(jī)輸出軸上的螺釘擰上以免丟失。圖20注意：旋轉(zhuǎn)電機(jī)的輸出臂直到它上面的孔和左側(cè)上臂上的孔完全吻合上，表面上看起來輸出臂上所有孔都一樣，事實(shí)上卻不同。17．用3顆#6-3/8英寸螺釘，彈墊和螺母，把型號(hào)為805BB的伺服電機(jī)裝到電機(jī)固定支架上如圖20所示。圖2018．用2顆#4-3/8英寸螺釘、鎖緊螺母，把電機(jī)輸出臂安裝到左側(cè)上臂彎曲部分的內(nèi)側(cè)如圖22和圖23所示。圖22圖2319．完成后的機(jī)械手上臂應(yīng)如圖24所示。在擰緊電機(jī)輸出軸上螺釘前，需確認(rèn)左側(cè)上臂能從垂直位置向前向后轉(zhuǎn)動(dòng)90度。圖2420．放置3顆#4-1/4英寸取間尼龍支柱到下轉(zhuǎn)動(dòng)托盤上如圖25所示。圖2521．用3顆1/2英寸#4螺釘，把圖24中所示的組件裝到下轉(zhuǎn)動(dòng)托盤上如圖26所示。圖26參考圖27來確認(rèn)組裝是否正確。圖27第五章組裝前臂1．用2顆1/4英寸螺釘、鎖緊螺母，把電機(jī)支架裝到前臂上，注意孔位相對(duì)如圖28所示，并通過前臂末端小孔固定電機(jī)支架，如圖29所示。圖28圖292．用1顆5/16英寸#4螺釘、平墊、彈墊和螺母，把較短的彈簧安裝到電機(jī)固定支架上。注意平墊和彈簧前端的小圈齊平緊貼，如圖30和圖31所示。圖30圖313．像圖32一樣套2個(gè)#4SAE平墊到臂上的鋁柱上，并涂適量的油脂到最后一個(gè)平墊的外側(cè)，注意讓平墊平的一側(cè)貼著臂上的構(gòu)件。圖324．用1顆9/16英寸#6螺釘、平墊、螺母，把圖31中所示的組建裝到圖32中所示的組件上如圖33所示。不要使勁擰緊9/16英寸的螺釘和螺母，擰到組件之間沒有空隙手臂能夠向上向下自由轉(zhuǎn)動(dòng)為好。圖33完成組裝后部件圖示圖34圖35用1/8英寸的鉆頭，把圖36中所示的孔擴(kuò)大。圖366．用2顆1/4英寸螺釘和鎖緊螺母，把電機(jī)輸出臂到上臂內(nèi)側(cè)如圖37所示（輸出臂平的一側(cè)貼著前臂）。圖3737．用2顆#6-3/8英寸螺釘，彈墊和螺母，把型號(hào)為HiTecHS-645MG電機(jī)裝到固定支架和機(jī)械手前臂上，如圖38和圖39所示。圖38圖398．裝完以后，把上臂上圓形電機(jī)輸出臂和前臂上電機(jī)輸出軸連接起來，要注意前臂能像圖40和41所示一樣向前向后自由地旋轉(zhuǎn)90度，之后再擰緊螺釘。圖40圖41第六章組裝手爪1．用2顆1/4英寸螺釘，把手爪的兩片連接起來，如圖42所示。圖422．把1/16英寸的橡膠材料粘到手爪上。并把橡膠材料剪到與手爪邊緣對(duì)齊，把遮住手爪上狹槽的橡膠材料挖去，后先暫時(shí)把手爪放到一邊。3.用2顆3/8英寸螺釘、鎖緊螺母把型號(hào)為HS475HB的電機(jī)安裝到圖43所示的構(gòu)件上。先別完全擰緊#6螺釘，在后面的步驟中擰緊更合適。圖43如果組裝SG5五自由度機(jī)械手完成第4到第5步。如果組裝SG6六自由度機(jī)械手完成第6到第11步。4．用2顆1/4英寸#4螺釘和鎖緊螺母，把電機(jī)支架組裝到上一步驟中組裝好的組件中，如圖44所示。注意垂直方向上電機(jī)支架上的狹槽和組件上的狹槽位置重合，見圖44和圖45。圖44圖455．用2顆#6螺釘彈墊和螺母，把型號(hào)為HS-475的伺服電機(jī)安裝到支架上如圖46所示。（注：圖46中顯示有四顆螺釘，安裝時(shí)只要用2顆安裝到對(duì)角線上就行了）。圖466．用1/8英寸的鉆頭，把型號(hào)為HS225MG電機(jī)上輸出臂上的兩個(gè)孔擴(kuò)大，如圖47所示。圖477．用2顆#4-5/16英寸螺釘，彈墊和螺母把電機(jī)輸出臂裝到支架上如圖48和圖49所示。圖48圖498．用2顆#6-3/8英寸螺釘，彈墊和螺母把型號(hào)為HS-225MG的電機(jī)安裝到支架上如圖50所示。在把電機(jī)裝到支架上時(shí)，注意電機(jī)的的方向。圖509．把型號(hào)為HS-225MG的電機(jī)裝到支架上如圖51和圖52。在繼續(xù)下一步之前得確認(rèn)電機(jī)可以從中心向左向右轉(zhuǎn)動(dòng)相同的角度。圖51圖5210．用1顆#4平墊和彈墊，把電機(jī)輸出軸上的螺釘擰到電機(jī)上如圖53所示。平墊在最里面，然后是彈墊再是螺釘。圖5311．用4顆#6-3/8英寸的螺釘，彈墊和螺母，把型號(hào)為HS475的電機(jī)裝到支架上，兩個(gè)電機(jī)中其中一個(gè)的安裝方法參照本章第3步到第5步。完成安裝后的組件如圖54和圖55所示。圖54圖5512．用2個(gè)#2彈墊、螺母把電機(jī)輸出臂裝到齒輪和其支架上如圖56所示，之后再把另一個(gè)齒輪也裝好。圖5613．把電機(jī)齒輪傳動(dòng)組件裝到電機(jī)支架上如圖57所示。在用螺釘固定齒輪支架之前注意它要能夠如圖57、圖58、圖59中所示靈活地轉(zhuǎn)動(dòng)。圖57圖58圖5914．用1顆1英寸#4螺釘，2個(gè)平墊，1顆鎖緊螺母和1個(gè)1/4英寸的取間尼龍支柱，把齒輪組件和電機(jī)支架組件組裝起來，如圖60所示。注意開始位置兩個(gè)齒輪組件的位置應(yīng)和圖61中指示的一樣。擰緊1英寸#4螺釘，讓組件中沒有間隙但能用撥動(dòng)。圖60提示：在平墊和鋁件之間涂適量的油脂，這樣運(yùn)轉(zhuǎn)起來時(shí)會(huì)更靈活。圖6115．用1顆3/8英寸#4螺釘，2平墊和1顆螺母，把手爪裝到齒輪傳動(dòng)裝置上如圖62和圖63所示。擰緊螺母讓組件間沒有間隙但能被手撥動(dòng)。圖62圖6316．用1顆1/2英寸#4螺釘，4個(gè)平墊和1個(gè)鎖緊螺母，把扁平的連桿裝到齒輪機(jī)構(gòu)上如圖64和圖65。還是擰緊螺釘?shù)浇M件之間沒有空隙但又能被手撥動(dòng)。圖64圖65圖6617．用1顆1英寸#4螺釘，2個(gè)平墊，2個(gè)1/4英寸取間尼龍支柱和1個(gè)鎖緊螺母，安裝扁平連接件到支架內(nèi)側(cè)，如圖67所示。圖6718．重復(fù)同樣的安裝循序把另一只手爪也組裝好如圖68和69所示。圖68圖6919．用2顆1/4英寸#4螺釘和鎖緊螺母，把傳感器支架安裝到手爪機(jī)構(gòu)上如圖70所示。

圖70在組裝手爪時(shí)可以參考下面的6副圖。圖71（SG5手爪）圖72圖73圖74張開狀態(tài)圖75握緊狀態(tài)注：如果手爪的末端不能夠像圖75一樣嚙合，只需要擰松齒輪上的電機(jī)，把電機(jī)往左移一些，轉(zhuǎn)動(dòng)一個(gè)或兩個(gè)齒輪就可以調(diào)整好手爪。圖76第七章裝手爪到前臂上1．如圖77所示八1個(gè)1/4英寸SAE平墊套到鋁柱上，在平墊的外側(cè)面涂少量潤(rùn)滑劑。墊圈平的一面必須靠前臂。圖772．把手爪組件裝到前臂，在固定組件之前需確保手爪組件能從中線開始各轉(zhuǎn)45度，如圖78。圖78第八章彈簧安裝和連接電機(jī)電纜1．調(diào)整前臂支架，使它離電機(jī)固定支架約1英寸遠(yuǎn)，如圖79所示。測(cè)量前水平放置組件。較長(zhǎng)的彈簧可裝可不裝，但當(dāng)用機(jī)械手提取較重物體或經(jīng)常要不斷地長(zhǎng)時(shí)間工作時(shí)，要求裝上長(zhǎng)彈簧。圖792．用1顆1.5英寸#4螺釘,1個(gè)墊圈和4個(gè)1/4英寸的取間尼龍支柱,如圖80所示,把長(zhǎng)彈簧牢固的連接上.圖803.用1顆#4-5/8英寸、平墊、彈墊、螺母和1/4英寸取間尼龍支柱，把長(zhǎng)彈簧的另一端到前臂上如圖81所示。擰緊螺釘時(shí)注意，彈簧末端的圈不能偏向左邊或右邊，應(yīng)居中。圖814．用1顆5/16英寸#4螺釘、墊圈和彈墊、螺母，把小彈簧裝到手臂上如圖82所示。擰緊螺釘時(shí)注意，彈簧末端的圈不能偏向左邊或右邊，應(yīng)居中。小彈簧是必須的，它可以抵消手臂前向的作用力使上的電機(jī)能夠達(dá)到提起的最大位置。圖825．把電機(jī)電纜從上臂和前臂支架上孔中穿過，在用扎帶扎緊電機(jī)電纜的之前比需確認(rèn)電纜不能影響手臂的正常運(yùn)動(dòng)。在扎緊之前可以水平放置機(jī)械手來確定電纜的長(zhǎng)度。6．電機(jī)電纜布置好后，用2根12英寸長(zhǎng)和2根6英寸長(zhǎng)的電纜延長(zhǎng)線連接電機(jī)輸出電纜，以便它能夠足夠長(zhǎng)與PSC電機(jī)控制器相連，（圖83）電機(jī)電纜與電機(jī)控制器的連接方法在后面有介紹。圖83

第九章教學(xué)板和電機(jī)控制器的連接下面的圖片是SG5五自由度、SG6六自由度機(jī)械手教學(xué)板BOE和電機(jī)控制器PSC之間典型的連接方法。結(jié)合圖87來具體了解伺服電機(jī)和電機(jī)控制器PSC之間的連接。圖86圖87第十章使用和操作機(jī)械手時(shí)必須注意的事項(xiàng)：在操作機(jī)械手前必須把機(jī)械手固定在一個(gè)平臺(tái)上（底座盒上有3個(gè)用來固定機(jī)械手的部件）。在編程或操作機(jī)械手時(shí)，最好讓離機(jī)械手3英尺的空間范圍內(nèi)沒有任何物體。當(dāng)停止運(yùn)行機(jī)械手時(shí)注意用手扶著，或在關(guān)閉電源時(shí)讓機(jī)械手水平放置。使用5V-6V電源，用過高的電壓會(huì)讓伺服電機(jī)過熱而損壞電機(jī)。一般來說機(jī)器人上使用5V-6V電壓的電源是正常的。5V-6V電源電壓或5V、4A的電源的比較理想。當(dāng)機(jī)械手第一次運(yùn)行時(shí)，必須注意檢查電機(jī)是否過熱。檢查手爪上電機(jī)是否過熱，如果電機(jī)過熱檢查手爪上的螺釘是否擰得過緊，這種情況經(jīng)常發(fā)生。在編寫程序時(shí)請(qǐng)不要設(shè)置手爪張開位置或握緊位置到手爪的極限位置，以免損壞電機(jī)和手爪。不要讓SG5/6機(jī)械手長(zhǎng)時(shí)間處在圖84所示前向位置，否則因手臂重量產(chǎn)生力矩使肩部電機(jī)過熱而損壞電機(jī)。當(dāng)機(jī)械手在未上電或在操作過程中靜止時(shí)，手臂的肩膀應(yīng)盡可能的垂直，長(zhǎng)的彈簧形變盡可能的小。如圖85所示。參照隨機(jī)械手配置的說明書正確連接這兩部分，也可以以本手冊(cè)最后的圖作一個(gè)參照。請(qǐng)勿使用教學(xué)板的電源來給電機(jī)供電。請(qǐng)使用一個(gè)與電路板電源獨(dú)立的電源給電機(jī)供電，電壓在5-6伏特，至少能提供4安培電流的電源，否則機(jī)械部不能正常工作。圖84不正確的靜止或保持位置圖85正確的靜止或保持位置第十一章機(jī)械手例程例程1電機(jī)調(diào)試'[Adjust_Bicept.bsp]'{$STAMPBS2p}'{$PBASIC2.5}''FileAdjust_Bicept.bsp'PurposeAlignupperpivotdeckservos'AuthorCrustCrawlerInc.(MikeGebhard)'E-mailsupport@'Started6/1/2005'Updated''Hardware:'(1)SG5/6-UTRoboticArm'(1)ParallaxBasicStampPModule'(1)ParallasBOE'(1)ParallaxServoController(PSC)''========================================================================='GettingStarted'========================================================================='PLEASEverifythePSCtojointconnectionsbelow.Theymightbe'differentfromyourassemblyguideinstructions.''BasePSCChannel0'BicepPSCChannel1'ElbowPSCChannel2'WristPSCChannel3'WristRotatePSCChannel4(SG6Connection)'GripperPSCChannel5'RightBicepPSCChannel6''========================================================================='ProgramOverview'========================================================================='Thisprogramisusedtoprogrammaticallyadjustthetwobicep'servosincasetheycannotbealignedphysically.Thisstep'iscrucialtoensurelongservolifeandlowcurrentconsumption.'Iftheservosarenotalignedproperly,theywillrunhotand'usemorecurrentthanwhat'srequiredfornormaloperation.''========================================================================='Aligningthebicepservos'========================================================================='1.Downloadthisprogram"asis"toyourbasicstamp.Bydefaultthe'bicepservosareprogrammedtocenter.Noticethedebugscreen'displays:''RightBicepOffsetConstantDeclaration:''RightBicepOffsetCON0''2.Lookforanydifferencesbetweenthebicepsservospositions.If'theyalignperfectlyyou'redone.Ifnotcontinuetostep3'physicaladjustments.''3.Physicallyadjustjustonebicepstomatchtheotherbicepsposition'asmuchaspossible.Removeonebicepfromtheservospindle,'rotateit,andsecureitbackontotheservospindle.''4.Lookforanydifferencesbetweenthebicepsservospositions.If'theyalignperfectlyyou'redone.Ifnotcontinuetostep5'Programmaticadjustments.''5.Findthissectionofcodebelow.'rightBicepPos=750'Changethe750valuetosomethingelse,like745,downloadthiscode'andlookforanydifferencesinthebicepservopositions.Continue'toprogrammaticallyadjusttherightBicepPosvariableuntilthe'bicepsareasclosetoparallelaspossible.''6.Writedowntheconstantdeclarationstatementonthedebugscreen.'YouwillusethisvalueinallSTAMPcodewrittenforthearm.'Thevaluecouldbeanegativenumber.'''Notes:'Servoscanvary15to20stepsfromeachother.TherightBicepPos'couldbe+/-20unitsfromtheotherbicepservo.Ifyourgetting'valuesabove20youshouldgobacktostep3andphysically'adjustthebicep.''#IF($stamp=BS2SX)OR($stamp=BS2P)#THENBaudCON1021'BS2p2400baud#ELSEBaudCON33164'BS22400baud#ENDIFPSCCON15'PSCModuleBicepCON1'LeftBicepPSCchannelRightBicepCON6'RightBicepPSCchannelrampCON$F'ServorotationspeedbicepPosVARWord'BiceptPositionrightBicepPosVARWord'RightBiceptPositiondiffVARWord'DifferenceinPosition'RecordyourRightBicepOffsetconstantdeclaration.RightBicepOffsetCON-10bicepPos=750'==================================================='AdjustthevaluebelowtoaligntheservosbracketrightBicepPos=750'===================================================Write_PSC:SEROUTPSC,Baud,["!SC",Bicep,ramp,bicepPos.LOWBYTE,bicepPos.HIGHBYTE,CR]SEROUTPSC,Baud,["!SC",RightBicep,ramp,rightBicepPos.LOWBYTE,rightBicepPos.HIGHBYTE,CR]diff=rightBicepPos-bicepPosDEBUG"RightBicepOffsetConstantDeclaration:",CR,"",CR,CR,"RightBicepOffsetCON",SDECdiffEND例程2手臂的基本動(dòng)作'[ArmEngine_Basic.bsp]'{$STAMPBS2p}'{$PBASIC2.5}''FileArmEngine_Basic.bsp'PurposeBasicarmmovement'AuthorCrustCrawlerInc.(MikeGebhard)'E-mailsupport@'Created5/26/2005'Updated11/14/2005''Hardware:'(1)SG5/6-UTRoboticArm'(1)ParallaxBasicStampPModule'(1)ParallasBOE'(1)ParallaxServoController(PSC)'''========================================================================='Updates11/14/2005IncludessupportforSG5&6Version2Roboticarms'========================================================================='CodeupdatedforSG5andSG6version2.0.SGversion2armshavea'singlebicepservo.IfyouhaveaSGversion2changethelinebelow'toread#DEFINEsgVersion=2andsavethefile.Ifyouhaveagray'armoranarmwith2servobicepservosleave#DEFINEsgVersion=1.'#DEFINEsgVersion=1''========================================================================='Updates9/24/2005IncludessupportforSG6(6axisarm)'========================================================================='ArmEngine_Basic.bspwillfunctionforboththeSG6andSG5,6and5'axisroboticarms.Ifyoupurchasea5axisyoushouldconsiderturning'offtherotatingwristbyeditingonelineofcode.Findthecode'sectionbelow.''Main:'...'baseBit=1'biceptBit=1'elbowBit=1'wristBit=1'wristRotateBit=1'SG5userschangeto0'gripperBit=1'ChangewristRotateBit=1assignmentto'wristRotateBit=0''SG5Users:'Thereisnoharminleavingthestatementasis.Doingsocauses'unnecessarylinesofcodetorun.Theresultmightlooklikea'slightdelayinprogramexecutionwhilethePSCsendsdatatoa'nonexistentjoint.''========================================================================='GettingStarted'========================================================================='1.VerifyyourPSCchannelconnections.'2.Verifythatyourarmisadjustedphysicallyand'programmatically.'3.Enterthe"RightBicepOffsetConstantDeclaration"youfound'runningtheAdjust_Bicept.bspcode.Thisispartoftheassembly'instructionsasof5/26/2005.Thiscodewillnotrununtilyou'completethisstep.Ifyoualreadyassembledthearm,thisvalue'isthedifferencebetweenthebicepservosfoundinChapter#5:'AdjustingtheBicepsServos.''========================================================================='ArmservojointstoPSCchannelsconnections'========================================================================='PLEASEverifythePSCconnectionbelow.Theymightbedifferentfromyour'assemblyguideinstructions.'Base0'Bicep1'Elbow2'Wrist3'WristRotate4-->(SG6only)'Gripper5'RightBicep6-->SGversion1only(GrayArm)''========================================================================='ProgramOverview'========================================================================='ThisisthebasecodeusedtomovetheSGarmjoints.Ittakesa'7elementarraycontainingservopositions(indegrees)andonecontrol'byte(ctrlByte)asparameters.Thearrayprovidesinstructionsto'positionthearmjoints.ThectrlBytecontrolswhichjointsmove.''Alljointsmovewithinadjustablecontraints.Constrainingthejoints'stopsthearmfrommovingintoanunrecoverableposition,allowsthe'usertocontrolwherethearmdoesitswork,and/orstopsajointfrom'movingtoapositionthatcouldpotentiallyharmaservoorservo'assembly.''Thiscodeisdesignedtogivedevelopersastartingplacesothatthe'developerdoesnotneedtorewriteordisassembleexistingcode.'Subroutinedetailsbelow''========================================================================='Walk-Through:'========================================================================='Move_Arm_JointschecksthebaseBit,biceptBit,elbowBit,wristBit,'andgripperBitvariablesrespectivelytodeterminewhichjointtomove.'A1meansmovetherelatedjointtothepositioninarray(x)'0meansskipthisjoint.''Forexample,letssayyouwanttomovethebaseto30degress'You,theprogrammer,wouldassignbaseBit=1andarray(Base)=30.'ThesubroutineseesthebaseBitequals1andknowsyouwantto'rotatethebase.''Next,jointconstraintsarecheckedbytheCheck_Joint_Constraint'helpersub.Ifthevalueinarray(pscChannel),array(0)inthisexample,'fallswithinthebaseservocontraints,nothinghappens(morelater).''Sincearray(pscChannel)isindegreesandthePSCdoesnotunderstand'degreesthearray(pscChannel)valueisconvertedtoPSCunitsbythe'Convert_Degrees_To_PSCUnitshelpersub.''AllvaluesaresenttoWrite_PSCwhichisresponsiblefor'sendingtheinformationtothePSCandmovingthebaseservo.''Move_Arm_Jointscontinuesbycheckingeachoftheremaining"jointBits"''Order:'1.baseBit'2.biceptBit'3.elbowBit'4.wristBit'5.wristRotateBit'6.gripperBit''========================================================================='SubRoutines.'========================================================================='Eachsubroutinecantakeinputparametersandreturnorupdate'variables.''Example:'sub(inputparameter<datatype>)returnvariable<datatype>.''Thenextseveralsectionsdescribeparameters,returnvalues,and'generalfunctionality.''========================================================================='Move_Arm_Joints'sub(array(6)<byte>,ctrlByte<byte>)'========================================================================='Move_Arm_Jointsisthemainsubroutineforcontrollingarmmovement.'Ittakesthefollowingrequiredparameters:''-ctrlByteThisbytecontrolswhichjointtomove.Itismadeupofthe'aliesesbelow.'baseBitVARctrlByte.BIT0'0=nochange'biceptBitVARctrlByte.BIT1'1=jointvaluechanged'elbowBitVARctrlByte.BIT2'wristBitVARctrlByte.BIT3'wristRotateBitVARctrlByte.BIT4-->SG6only'gripperBitVARctrlByte.BIT5''-array(6)containsdegreepositionsforall(5or6)armjointsvalues.'Thesevaluesrangefrom0to180degreesormaximunservo'rotation.Itisuptotheprogrammertoverifythatthees'valuesdonotexceed180elseyouwillgetunexpected'results.''array(Base)'array(Bicep)'array(Elbow)'array(Wrist)'array(WristRotate)-->SG6only'array(Gripper)''========================================================================='Check_Joint_Constraint'========================================================================='sub(pscChannel<nib>)returnarray(pscChannel)<byte>''-pscChannelThisvariableholdsthePSCchannelrelatedtothecurrent'jointtomoveandthearrayelementthatholdsthejoint'position.''Check_Joint_Constraintismorelikeapropertyofthearm.Bysuppling'contraintstothearmyoudefinewherethearmdoesitsworkorlackof.'ConstraintinformationisstoredinScratchPadRAM(PUTCommand).Each'armjointhasarelatedlowerandupperconstraint.Thesevaluescan'easilybechangedprogrammaticallyduringprogramexecutionsothework'areacanchangedynamicallyduringruntime.''Let'slookatthebaseservoconstraints.Firstfindthelineofcode'below.Thefirsttwonumbers30and150arethebaseservocontraints.''basebiceptelbowwristwristRgripper'PUTConstraints,30,150,60,140,0,150,0,170,0,180,90,170''WhentheCheck_Joint_Constraintiscalleditgrapsthelowerand'upperbasecontraints(Constraints+(pscChannel*2))andplacesthem'inlowerConstrandupperConstrvariables.'GET(Constraints+(pscChannel*2)),lowerConstr,upperConstr''Thevalueinarray(pscChannel)<pscChannel=Base>ischeckedagainst'thecontraints.Ifarray(pscChannel)islessthanthelowerconstraint'thenarray(pscChannel)=lowerContr.Ifarray(pscChannel)isgreater'thanupperConstrthenarray(pscChannel)=upperConstr.Ifneitherare'truethenarray(pscChannel)isnotmodifiedatthistime.''========================================================================='Convert_Degrees_To_PSCUnits'========================================================================='sub(pscChannel<nib>)returnservoPos<word>''-servoPosThisvariableholdstheservopostioninPSCterms''ThisfunctionconvertsdegreestoPSCunits'servoPos=((angle*56)/10)+250''========================================================================='Convert_PSCUnits_To_Degrees'========================================================================='sub(servoPos<byte>)returnarray(pscChannel)<byte>''ThisfunctionconvertsPSCunitstodegrees.Thisfunctionisnotused.'Itisincludedforuseinlaterapplications.'array(pscChannel)=((servoPos-250)*10)/56''========================================================================='Write_PSC'========================================================================='sub(PSCChannel<byte>,ramp<byte>,servoPos<word>)''Write_PSCisresponsibleforsendingjointinformationtothePSC.The'resultisajointmoves.''========================================================================='Display'========================================================================='sub()returns<allvariables>''Thissubdisplaysallthevariablesandvaluesusedinthiscodewhen''#DEFINEdebugMode=1''Thisisahandytroubleshootingtechniqueifyoudecidetochangethe'basecodeandneedtoknowwhatthevariablesaredoing.''Formoreinformationlookup"compilerdirectives"inthePBASIChelp.''========================================================================='Main'========================================================================='Thissubisdesignedtogiveaprogrammerhandsonexperiencewith'thevariablesandconstantsusedinthisprogram.Likethemovebase'exampleabove,ifyouwanttomovethebaseandonlythebaseservo'to30degreesdothefollowing.''Main:'array(Base)=30'array(Bicep)=90'array(Elbow)=90'array(Wrist)=90'array(WristRotate)=90-->SG6only'array(Gripper)=120'ctrlByte=%00000000'Resetcontrolbyte'baseBit=1'biceptBit=0'elbowBit=0'wristBit=0'wristRotateBit=0'gripperBit=0''Note:OnlythebaseBit=1.Thereforeonlythebaseservorotates.''Runtheprogram.''[I/O]#IF($stamp=BS2SX)OR($stamp=BS2P)#THENBaudCON1021'BS2p2400baud#ELSEBaudCON33164'BS22400baud#ENDIFPSCCON15'PSCModuledegreesCON2'AmounttomoveajointBaseCON0'RotatingbaseBicepCON1'LeftbiceptElbowCON2'ElbowWristCON3'WristWristRotateCON4'RotatingWristGripperCON5'GripperRightBicepCON6'Rightbicept'================================================================='Constantdeclarationstatementfoundfrom'runningAdjust_Bicept.bspduringassembly.'Thiscodewillnotdownloaduntilyoudeclare'theRightBicepOffsetconstant.'Chapter#5:AdjustingtheBicepsServos.#IFsgVersion=1#THENRightBicepOffsetCON'-10'RightBicepoffset#ENDIF'=================================================================ConstraintsCON0'StartofconstraintsRC_OnCON1'ReceiverONRC_OffCON0'ReceiverOFF'[Variables]channelValueVARWord'RxchannelvalueservoPosVARWord'PSCservopositionarrayVARByte(6)'Servopositions(degrees)lowerConstrVARByte'lowerangleconstraintupperConstrVARByte'upperangleconstraintrampVARByte'servorotationspeedpscChannelVARNib'PSCchannelRxChannelsVARNib'ReceiverchannelRCVARBit'RxOn/OffctrlByteVARByte'Servojoint(s)updatedbaseBitVARctrlByte.BIT0'0=nochangebicepBitVARctrlByte.BIT1'1=jointvaluechangedelbowBitVARctrlByte.BIT2wristBitVARctrlByte.BIT3wristRotateBitVARctrlByte.BIT4gripperBitVARctrlByte.BIT5''Jointconstraintsinangles'Thevaluesbelowlimitwherethearmjointscanbepositioned.'Theconstraintsstopthearmfromgettingitselfinto'aunrecoverablepositionorapositionthatcoulddamage'aservo.Forexamplethebaseservocanmovefrom30'to150degreesnomoreandnoless.''basebicepelbowwristwristRgripperPUTConstraints,30,150,60,140,0,150,0,170,0,180,90,170#DEFINEdebugMode=0ramp=$BMain:array(Base)=90array(Bicep)=100array(Elbow)=90array(Wrist)=90array(WristRotate)=90array(Gripper)=120ctrlByte=%00000000'ResetcontrolbytebaseBit=1bicepBit=1elbowBit=1wristBit=1wristRotateBit=1'SG5userschangeto0gripperBit=1GOTOMove_Arm_JointsEND'sub(array(6)<byte>,ctrlByte<byte>)Move_Arm_Joints:'MovebaseifbaseBit=