輕松DIY擊鼓機器人,提供詳細軟硬件設計參數和系統(tǒng) 解決方案
1. 基本組成部分概述
1.1 機械結構部分
【機械臂主體結構】
機械臂主體全手工制作,由15種零件,合共25件零件構成,其中包括3根規(guī)格不同的軸,2種規(guī)格不同的軸承合計5個,支架1種合共2件,支座1種合共1件,第一級機械臂1種合共2件,第二級機械臂1種合共2件,第三級機械臂1種合計2件,機械臂間連接件3種共4件,軸固定件2種合計4件,另有用于各個零件間固定的角鐵合計8件。其中除軸承外均為鋁合金制件,機械臂手臂部分由平行鋁合金板構成雙立式結構,第三級機械臂共有兩邊,分別可自由活動,其由單塊鋁合金板組成,一端直接與舵機相連,另一端固定鼓棒。軸分為豎直軸與水平軸,豎直軸與步進電機相套,水平軸由軸固定件與軸承分別連接各級機械臂。
整體呈現單支雙臂式結構,合共有5個自由度,其中3個自由度有電機驅動,其中位于底座下方的步進電機用于轉動,而位于第二級與第三級機械臂之間連接的舵機用于帶動第三級機械臂轉動。鋁合金板零件之間使用自制淬火角鐵進行固定,避免對鋁合金制件進行鈑金工作,以減低成本和制作時間;軸與鋁合金工件之間采用軸連接件與軸承混合固定的方式進行配合與固定。整體機械臂合共使用了將近200g的螺絲與螺母,主要用于角鐵與鋁合金板件的緊固處理,另外也有使用少量緊固螺釘進行軸與固定件或者電機之間的固定。舵機上面也裝有四個帶有齒孔的固定件,用于夾持第三級機械臂。(詳見圖1.1)
圖1.1 機械臂主體結構實物圖
【底座結構】
機器人底座由2塊平行有機玻璃構成,其中由4根螺柱連接2塊玻璃板。面板裝有軸承,通過軸承自帶的小孔與有機玻璃地板進行螺絲固定。底板通過角鐵夾住步進電機,使步進電機自身不能轉動,豎直軸穿過軸承與步進電機相連,豎直軸底部為空心處理,并且使電機軸套入豎直軸中,并使用緊固螺釘進行兩者的固定。面板同時有9根固定螺釘,用于支撐鼓。2塊底板之間的空間用于放置主控制電路電路板PIC,步進電機驅動及其中一個電源。(詳見圖1.2及1.3)
圖1.2 底座部分內部實物示意圖
圖1.3 底座部分整體實物圖
1.2 電子電路部分
【整體電路系統(tǒng)】
整個機器人的電路系統(tǒng)由3個主要部分組成分別為由PIC組成的主控制電路,由聲音傳感器與觸碰開關組成的子控制電路,以及由單片機控制的電機及其配套電路,其中電機及其配套電路又細分為舵機的直接控制與使用步進電機驅動電路進行對步進電機的控制。
【主控制電路】
機器人主控制電路由開發(fā)板和最小系統(tǒng)板組成,開發(fā)板主要構造包括有PIC單片機,時鐘電路系統(tǒng),蜂鳴器電路,復位電路系統(tǒng),配套的LED燈顯示系統(tǒng),數字顯示系統(tǒng),外接數據線系統(tǒng)等組成(見圖1.4),外有與PIC匹配5V電壓電源系統(tǒng)相連,向主控制電路提供穩(wěn)定的電壓。利用主控制電路上的排針設備,可以使用電路連接線連接各個子控制電路、舵機與步進電機驅動器的相關接口,通過主控制電路接收子控制電路的反饋信息,通過對應的判斷向舵機、步進電機輸出相應的脈沖,控制電機運動。其控制流程如下:
圖1.4 主控制電路控制流程示意圖
【子控制電路】
機器人子控制電路由手工制作的電路板組成,其中子控制電路又分為聲控子電路與觸控子電路;其中聲控電路部分主要由聲音傳感器及指示燈系統(tǒng)構成,能夠接收環(huán)境變量中的聲音分量,通過調節(jié)可變電阻可以使聲音傳感器接收合適范圍的聲強,并通過整波電路與放大電路將相關的信息傳遞到單片機中;觸控電路主要部分由觸碰開關構成,觸碰開關將在設計中外接,通過擊打觸碰開關,使開關通過電流,通過電流后整體經過各類比較器、放大器、整波元件實現向PIC單片機輸出有高電平經過的信息功能。其控制流程如下所示:
圖1.5 子控制電路整體示意圖
【步進電機驅動電路】
步進電機驅動電路為現成集成模塊,外部與15V電壓相連,邏輯電源接單片機VCC,通過單片機輸出脈沖實現一定角速度轉動,同時由DIR(方向)接口決定步進電機轉動方向,FREE(使能)接口決定步進電機的啟動與停止速度。
2. 基本功能概述
【擊鼓功能】
機器人通過第三級機械臂及其連接的鼓棒實現擊鼓的功能,其中第三級機械臂由舵機帶動,調節(jié)舵機的高低電平延時時間實現對舵機的轉速、擊打力度的控制,通過高低電平輸出的循環(huán)實現轉角的控制,同時調用相關的程序,使機器人可以按照一定的節(jié)奏或者現場示教人員的指示進行有規(guī)律的擊鼓動作。
如下圖所示,機器人當給予舵機延時2400us的高電平以及20ms低電平的脈沖信號(詳見圖3.5)并且將該程序循環(huán)40次,舵機會從圖1.9所示位置A移動到圖1.10位置B,同時通過第三級機械臂、舵機自身、鼓棒形成一個三級結構,擊打鼓,使鼓發(fā)出一定的響聲;而當給予舵機延時600us的高電平以及20ms低電平的脈沖信號(詳見圖3.7)并且將該程序循環(huán)40次,舵機會從圖1.6所示位置回復到圖1.5所示位置,并且通過其自身的機械零點進行回復。
通過單片機預先編寫的程序或者示教的模式,機器人按照一定的規(guī)律進行擊鼓,其擊鼓的頻率約為0.7-1.0秒/次,其中下行時間約為0.3-0.5秒,上行時間約為0.4-0.6秒,通過調節(jié)左右舵機輸出不同的脈沖信號,可以實現交替擊打(見圖1.7,圖1.8),交替極大能夠實現整體機器人平均擊鼓頻率在0.3-0.5秒/次,而通過雙臂僅能同時使整體機器人的擊鼓頻率在0.8-1.0秒/次左右。
而步進電機能夠使機器人通過自身的轉動,擊打到不同位置的鼓,通過對步進電機的輸出脈沖的個數進行控制,根據步矩角理論則能夠使步進電機轉動一定的角度,經過計算可以測出擊打左側兩個鼓與擊打右側兩個鼓需要轉動的角度,即循環(huán)的次數,使步進電機轉動要求的角度,并且實現擊打的鼓的機械轉換。
圖1.6 擊鼓功能控制流程圖
圖1.7 機器人同步擊打A位置實物示意圖
圖1.8 機器人同步擊打B位置實物示意圖
圖1.9 機器人異步擊打實物示意圖A
圖1.10 機器人異步擊打實物示意圖B
【離線編程功能】
通過在預先在PIC單片機中離線編寫好的程序,程序中進行對舵機與步進電機的脈沖信號輸出,其中包括有舵機的異步控制方法、同步控制方法以及步進電機的控制;異步控制方法為單片機對兩個舵機中的一個輸出脈寬為2.4ms的高電平和20ms低電平而對另外一個電機輸出脈寬為0.6ms的高電平和20ms(詳見圖1,13),循環(huán)一定次數之后交換兩個電機的輸出脈沖(詳見圖1.14),這樣兩個舵機將會出現一個左側機械臂下行同時右側機械臂上行,然后左側機械臂上行右側機械臂下行的交替擊打;同步控制方法為PIC單片機對兩個舵機同時輸出脈寬為2.4ms的高電平和20ms的低電平(詳見圖1.15)循環(huán)一定次數之后同時再輸出脈寬為0.6ms的高電平和20ms的低電平,這樣兩個舵機將會出現同時擊打同時復位的情況。步進電機使用占空比為50%的高低電平調控,能夠實現一定的步矩角的轉動。
離線編程系統(tǒng)通過多次重復地不同地調用舵機異步控制、舵機同步控制、步進電機控制的子程序,能夠實現機器人在步進電機一邊轉動一邊舵機同步擊打、步進電機一邊轉動一邊舵機異步擊打,步進電機停動同時舵機同步擊打、步進電機停動同時舵機異步擊打四種主要情況,通過交替、多次、重復、并排使用這些子程序和情況,能夠使機器人整體在圍繞三個鼓之間進行多次的來回往復擊打,同時能夠進行左右臂同時擊打(見圖1.7,圖1.8)和左右臂交替擊打的控制(見圖1.9,圖1.10)。
圖1.11 兩舵機異步控制脈沖信號示意圖A
圖1.12 兩舵機異步控制脈沖信號示意圖B
圖1.13 兩舵機同步控制脈沖信號示意圖
【示教功能】
通過在聲音傳感子控制電路,通過可變電阻調整聲音傳感器的接收范圍,可以識別現場的聲音,機器人通過從子控制系統(tǒng)反饋的聲音信息,能夠實時輸出相應的舵機同步控制脈沖信號,舵機同時擊鼓,機器人跟隨聲音進行運動;通過觸感子控制電路,可以識別人是否對示教的模擬鼓進行擊打,如果是則輸出相應的脈沖信號使一側舵機進行跟隨運動。
二、目的與意義
1. 創(chuàng)作意圖與靈感來源
【創(chuàng)作意圖】
大白菜作為一個擊鼓機器人,其主要功能并不局限與擊鼓。我們創(chuàng)作這個機器人的意圖是作為后續(xù)項目的前期制作,對后續(xù)項目中使用到的機械手進行理論的論證,以及對其進行部分制作,大白菜是后續(xù)項目中機械手的簡化版本,我們也在大白菜上預留足夠的空間以及增加電機的空間,用于日后在項目中對其進行改裝與增加自由度。
【靈感來源】
大白菜的靈感來源于多個方面:
(1)、基本結構:為了適應后續(xù)項目中的仿人式機械手而采用的單支雙臂式結構,第一級機械臂與第二級機械臂也采用與人相似的設計;
(2)、基本功能:由于在比賽中強調可展示性,強調能夠調動現場氣氛的能力,因此簡化了原有紀錄位姿的功能,單純采用單片機的控制而取消對電機轉角、轉速的測量,從而萌生使機械手進行擊鼓動作的設計;
(3)、附加功能:包括聲音控制與觸感控制,則源于在最近結束的上海世博會中在上海企業(yè)聯合館中的等待區(qū)節(jié)目,第一個為由觀眾現場的拍掌,使場館的LED燈根據現場拍掌的節(jié)奏而閃亮,第二個為由現場觀眾喊出相應的字母,機器人撿起對應的箱子,仿照這2個節(jié)目我們設計出通過現場觀眾拍掌的聲音控制機器人以及通過現場擊鼓的展示使機器人紀錄示教人的擊打信息并重現的附加功能。
2. 項目目的
(1)、作為一款參加機器人比賽展示類的機器人,該機械臂能夠進行基本的擊鼓動作;而在附加功能上,則通過不同的傳感系統(tǒng),記錄示教人員的擊鼓節(jié)奏,并在示教完成之后機器人能夠重現該擊鼓的動作以及節(jié)奏。
(2)、作為一種單支雙臂式的機器人(機械臂),其仿人的設計用于作者對該理論的一種驗證,通過對仿人的設計,使機械手能夠模仿人的運動,而大白菜僅是作為對后期仿人機器人的設計與制作的一個前期步驟進行相關的研究與制作。
(3)、作為一個預留了大量改進空間的機器人,該機器人可以在多個方面進行不同的改進,包括預留增加自由度的位置,增加電機的位置,增加編碼盤測量電機轉角轉速,縮減底座面積等多個改進方向,通過不同方面改進能夠為日后研究機器人(機械臂)的運動、仿真、測量、多機器人協作等擴展功能提供了大量的現實數據、機器人實體和空間。
3.項目意義
(1)作為大賽的參賽作品,本機器人的最大功能與意義在于能夠實現最基本的擊鼓功能。并在擊鼓功能的基礎上增加附加功能,實現以離線編程模式與示教模式控制機器人的功能。能夠為觀眾帶來對機器人的一種新的感受,一種新的樂趣,并且讓更多觀眾參與到機器人當中。
(2)作為一種單支雙臂式的機器人(機械臂),本機器人的意義在于能夠通過舵機對其控制,實現擊鼓的功能;同時,也是作為對該類機械臂的結構的一種新思考的實踐與驗證。
(3)作為一個預留大量改進空間的機器人,本機器人的意義在于為后續(xù)的設計與制作提供大量的現實數據與機器人實體,并且為日后的各個探究方面的選擇提供了更多的參考數據資料,能夠使日后項目組成員的后續(xù)探究設計更為完善與周詳。
4. 可行性總結
【技術可行性】
表1 探究過程中應用的技術及使用范圍
應用領域 | 技術名稱 | 技術功能 |
機械設計 | CAD制圖 | 交付制造機械臂主體 |
三維建模軟件 | 基本示意及大致分析 | |
一般力學分析 | 測算自由度受力 | |
機械制造 | 機械加工 | 制造機械臂主體 |
一般鉗工 | 組裝及固定整體機器人 | |
自動控制 | 單片機控制 | 整體機器人控制 |
電機伺服控制 | 控制電機以期望速率、幅度運動 | |
電子電路 | 傳感器 | 接收環(huán)境變量信息 |
電路處理 | 放大、優(yōu)化、整理電路信息 |
上述各種技術已經全部成熟及成型,只需要在掌握這些技術的基礎上使用它們,并不需要通過自行設計技術來實現;同時并且通過使用這些技術,實現機器人的每個部分細節(jié)的處理,就能夠最終完成機器人的整體制作與調試,整個機器人的制作中使用的技術都是可行的。
【應用可行性】
本機器人作為一款基本參加大賽的作品,在參賽的應用方面由于其擁有良好的展示性與娛樂性,該方面應用是可行的;另外,由于預留了大量的改進空間,即便是在比賽之后,需要進行多方面的改進,在未來,仍然能夠使用這個機器人現有的本體作后續(xù)的研究,該方面應用同時可行。
三、技術參數
1. 機械部分
【整體質量與尺寸參數】
(1)、整體質量:2.5Kg
(2)、整體尺寸:40cm X 30cm X 40cm (長X寬X高)
【機械工作參數】
(1)、整體工作最大寬度:49cm
(2)、整體工作最大高度:53cm
(3)、整體工作最高溫度:45℃
(4)、整體工作最低溫度:-10℃
(5)、舵機工作最大轉角幅度:90°
(6)、步進電機工作最大轉角幅度:30°
【電機參數】
表2 步進電機的物理參數
步進電機型號 | 57H46403(帶減速) |
步矩角 | 1.8° |
最大轉動角 | 360° |
相數 | 2 |
相電流 | 2.4A |
靜力矩 | 7 |
定位力矩 | 2.8 |
表3 舵機的物理參數
舵機型號 | TOWER PRO MG945 (輝盛) |
靜力矩 | 1.2 |
最大轉速 | 0.25sec/60°(4.8V) |
最大轉角 | 185° |
工作電壓 | 4.8-7.2V |
【機械臂主體力矩參數】
(1)、自由度計算
將支架和臂簡化為桿件,由于結構對稱性,將兩舵機旋轉副、每個軸的旋轉副和兩三級臂各只作一個,另外由于有兩個旋轉副(兩個軸所在的旋轉副)被限制,因此簡化結果如圖3.1:
圖3.1 機械臂全自由度簡化示意圖
活動組件n=2高副P =0,低副P =2,自由度為:
(1)
兩個自由度分別由舵機和步進電機控制,滿足機構穩(wěn)定要求。
(2)、舵機靜力矩校核
取鋁合金密度為:
(2)
將三級臂上孔去掉,簡化為一實體長方體塊,則三級臂體積為:
(3)
由(2)、(3)式數據可得三級臂質量為:
(4)
三級臂重心在其幾何中心,距離舵機旋轉軸距離為:
(5)
敲擊棒質量取 ,其重心距舵機旋轉軸距離為:
(6)
當三級臂和敲擊棒處于圖示水平時,舵機旋轉軸所需靜扭矩最大,其簡化結構圖見圖3.2
圖3.2 第三級機械臂長度簡化示意圖
可以得第三級機械臂的靜力矩為:
(7)
(8)
(3)、整體穩(wěn)定性校核
計算施加在軸2上扭矩
取該部分臂前端角鐵和螺釘、舵機總質量m =120g,二三級連接板尺寸:
(9)
其質量為:
kg (10)
同樣當三級臂水平時,如圖3.3,所施加的靜力矩為最大
圖3.3 第二級、第三級機械臂靜力矩簡化示意圖
此時重心距軸為L =100mm,將前端角鐵和螺釘、舵機總重心距軸2距離也取L =100mm
(11)
(12)
取 =0.25
底座圓盤半徑R=66mm,厚t=8mm,質量為:
(13)
kg (14)
假設整體受軸2上扭矩而傾斜,角度很小,則底盤邊緣一點與地接觸,此時,校驗要使整體不倒條件 ,設所需整體部分結構(底盤至軸2間部件)質量為m,其重心在整體中心軸上,如圖3.4所示
圖3.4 假定結構傾斜受力簡化示意圖
當傾角a很小時,底座重心、部分結構重心距傾角原點處距離均為R,列平衡條件:
(15)
k為動載荷扭矩時相對于靜載荷比值 ,k取大越穩(wěn)定,取2
(16)
由(16)式可得m>0.45kg
利用UG計算得實際m約0.70kg,這樣在底座、底座至軸部分結構重力力矩作用下,系統(tǒng)會有擺正趨勢,即a變小,可以保證要求,而且在中心軸作用下,整體會更穩(wěn)定不倒
2. 電子電路部分
【聲音傳感器】
表4 聲音傳感器模塊主要技術參數
尺寸 | |
主要芯片 | LM393,駐極體話筒 |
工作電壓 | 4V-6V |
【步進電機驅動器】
表5 步進電機驅動器技術參數與接口
步進電機驅動型號 | QDQ241 |
驅動最大電流 | 2.1A |
驅動最大相數 | 2 |
可選細分數 | 1/2,1/4,1/10,1/15 |
工作電壓 | 0-32V |
(1)、+COM 接單片機VCC5V,如果接24V,則CP,DIR各串接一個2K電阻 | |
(2)、CP 接脈沖頻率輸入,決定快慢 | |
(3)、DIR 方向控制,0V反轉,空正轉,決定方向 | |
(4)、FREE 使能接口,接0V停轉,電機自由狀態(tài) | |
(5)、A+ A相正輸入 | |
(6)、A- A相負輸入 | |
(7)、B+ B相正輸入 | |
(8)、B- B相負輸入 | |
(9)、+DC 接外接驅動電源,9-32V | |
(10)、 接0V |
【電源】
(1)、邏輯電源電壓:5V
(2)、步進電機驅動外加電壓:15V
3. 控制部分
【舵機輸入脈沖信號】
圖3.5 舵機正轉脈沖信號示意圖
圖3.6 舵機反轉脈沖信號示意圖
【步進電機輸入脈沖信號】
圖3.7 步進電機脈沖信號控制
四、使用說明
1. 常用控制與使用說明
【常見按鈕與連接】
圖4.1 步進電機驅動與電源連接方式實物示意圖
【使用注意事項】
(1)、避免使機器人在傾斜面上工作,以免從高處跌落造成損壞;
(2)、機械臂運動時請勿用手觸摸工作部分;
(3)、出現電機在通電狀態(tài)卻不工作的情況,請立刻切斷電源,以免電機損壞;
(4)、聲控開關和觸控開關為易損部件,請勿暴力操作;
2. 模式使用說明
聲控模式合適所有用戶使用,使用時將機器人擺在平臺上,平臺面積應足夠大,以確保機器人擊鼓時產生的震動不會使機器人跌落平臺。檢查電路連接是否有斷線、短路。確保無誤后接通電源,人站在附近1m處,雙手合拍,觀察機器人是否出現敲鼓動作,如果機器人沒有反應,則掌聲可以拍大一點,直至觀察到機器人執(zhí)行敲鼓動作。若此時讓機器人擊鼓所需掌聲還比較小,人可以離機器人動作遠一點拍掌觀察機器人是否敲鼓。這樣即可調節(jié)好人使用該機器人的掌聲大小和距離范圍。每次拍掌機器人會敲鼓一次,也可連續(xù)拍掌,但連續(xù)拍掌時速度不可過快,以留機器人反應時間。拍掌頻率掌握好后,人可以改變拍掌頻率,機器人則會以相同頻率做出擊鼓動作。如果人根據一首歌曲節(jié)奏連續(xù)拍掌,機器人將記憶該節(jié)奏并在人拍掌后擊鼓重現該節(jié)奏,從而實現娛樂功能。使用完畢后應及時關閉電源,以免電路元件發(fā)熱影響機器人使用壽命。
【觸感模式開啟與使用】
觸控模式可以合適所有用戶使用,使用時將機器人擺在平臺上,平臺面積應足夠大,以確保機器人擊鼓時產生的震動不會使機器人跌落平臺。檢查電路連接是否有斷線、短路。確保無誤后接通電源,單手觸摸一下左臂上任一個觸摸開關,觀察機器人左擊鼓手是否執(zhí)行擊鼓動作。同樣方式檢查右手擊鼓功能是否正常。若左右擊鼓手均正常,則可以每次觸摸機器人任一擊鼓手上的觸摸開關,使機器人執(zhí)行擊鼓動作。也可同時雙手觸摸兩擊鼓手觸摸開關,左右擊鼓手同步執(zhí)行擊鼓動作。人可以通過調整觸摸頻率、觸摸位置來使機器人打出富有旋律和動感的擊鼓聲。使用完畢后應及時關閉電源,以免電路元件發(fā)熱影響機器人使用壽命。
【再離線編程功能使用】
再離線編程功能主要由高級用戶使用。使用時請將電路的主要連接線全部移開,使PIC單片機所在的主控制電路板獨立出來使用,并同時打開您的電腦,調試您電腦的串口線或者其它燒錄PIC單片機的設備處于正常工作狀態(tài)。在確保您能夠向單片機燒入程序的情況之下,您可以開始編寫程序,我們建議使用常見的KEIL軟件進行單片機程序的編寫和項目的建立和調試。程序由主要幾個部分組成,我們建議您盡量不要對我們附上的程序的主要結構、系統(tǒng)、頭文件、舵機和步進電機的延時時間等方面進行更改,否則會導致機器人的某些部分或者整體無法工作;請您對如各個舵機、步進電機的子程序進行某些次序的變化,或者對舵機的循環(huán)次數進行修改,這樣可以根據您的喜好,調節(jié)機械臂的包括擊鼓的轉動角度范圍、步進電機的轉動范圍以及舵機交替擊鼓與同步擊鼓的次序進行調整,使機器人的表演更加具備您個人的特色。使用該功能的時候請您特別注意PIC單片機的數位與存儲范圍。完成燒錄之后將電路重新接回,開啟離線編程模式之后機器人就能夠按您的設定進行運動。
3. 常見問題與解決
【常見問題及解決QA】
(1)Q:為何通電之后PIC單片機指示燈不亮,同時驅動器指示燈微亮?
A:請您確定您已經給兩個電路接入正確的電壓,PIC單片機接的是5V的邏輯電壓,電機驅動器接的是15V的外加電壓,如果二者弄反了,可能會導致邏輯電路燒壞的情況。而驅動器在低于9V的電壓條件下不能拖動步進電機工作,因此請保證您已經為機器人接入正確的電源。
(2)Q:我已經接入正確的電源,為何舵機不能工作?
A:請您確定您現在使用的模式是什么,如果您使用的是離線編程模式,可能是由于PIC單片機內沒有預設的程序,或者是您在本機器人再離線編程系統(tǒng)使用中給舵機輸出的脈沖信號不正確,本舵機使用的是伺服控制技術,高電平延時2.4ms,低電平延時20ms為舵機正轉,高電平延時0.6ms,低電平延時20ms時為舵機反轉,一但延時時間不足,可能導致舵機不能工作;如果您使用的是聲控模式或者觸控模式,請確定您已經正確開啟這些模式,通過聲控系統(tǒng)指示燈和觸控系統(tǒng)指示燈可以判斷系統(tǒng)是否接收到您所擊打的信號。
(3) Q:我已經接入正確的電源,為何步進電機不能工作?
A:請您確定你給步進電機輸出的脈沖是正確的,如果您使用再離線編程系統(tǒng)給步進電機輸出的脈沖信號不正確,脈沖頻率過低會使步進電機轉動不明顯或者不轉動,脈沖頻率過高會使步進電機發(fā)生跳動并產生刺耳的高頻聲音。本步進電機的工作頻率幅度比較大,為了保證足夠的觀賞性和安全性能,建議您使用高低電平交替的方波信號,兩者延時時間均為1.2±0.5ms
(4)Q:為何我的舵機揮動手臂但打不到鼓?
A:如果您使用了再離線編程模式,請您確定您對舵機輸出的脈沖的循環(huán)次數,如果輸出的脈沖循環(huán)次數過低,會導致舵機的轉動角度大幅減少,這樣機器人就無法擊鼓了。
(5)Q:我明明已經給機器人聲音信號,為何機器人仍然沒有反應?
A:請您首先確定您使用的聲控模式,如果不是,請將其調至聲控模式再給予聲控信號;若您確定已使用聲控模式,請您首先觀察聲控模式的指示燈在您擊打鼓的時候有亮,如果沒有亮的話則可能是您給予的聲音信號不夠強,此時可以通過調節(jié)聲音傳感器一側的可變電阻以調節(jié)其靈敏度;如果燈在您擊打的時候有亮,但電機仍然不轉動,則表示PIC單片機在接收到信號的條件下無法帶動電機,此時請檢查您是否曾使用過再離線編程功能,是否對舵機的驅動子程序進行過修改,如是則請按后面提供的源程序列表恢復;如果您不曾使用過在離線編程功能,則可能是由于電路方面連接出現問題導致。
五、探究過程
1. 設計思路
大白菜的靈感來源與我們對機械臂方面研究的興趣,整個探究過程的設計采取以一個中心點出發(fā)進行擴展的設計思路,其思路表達如下:
圖5.1 設計思路流程圖
2. 探究過程簡述
首周作為確定題目和參賽的一周,主要工作在于對確定參賽的類型,項目組成員進行相關的構思與設計,盡管已經擁有一個確定的方向,但是仍然需要確定設計的整體思路與方案,以及進行接下來工作的一些物資與資料準備。
次周作為機器人制作的開始,主要工作在于機械設計方面,尤其是以機械臂的主體結構進行CAD及UG等軟件方面的設計為重,同時對機器人進行相應的受力分析以及對應的細節(jié)處理,并將初稿提交給指導老師進行審定與修改;電路方面進行初期的購買物資,確定元器件類型與數量,對開發(fā)板與最小系統(tǒng)板進行對應的初步調試。
機械機構部分 | 電路與控制部分 |
CAD與UG軟件制圖 受力分析 細節(jié)處理 指導老師修改機械設計方案 | 確定元器件類型與數量 購買物資 調試PIC單片機開發(fā)板 |
第三周工作進入全面開展的狀態(tài),由于機械設計方面出現不少的問題,需要多次修改機械設計部分,如軸、各級機械臂、連接件等方面都出現了如孔徑不均、鈑金件尺寸不規(guī)范、軸沒有分級等多種問題,需要多次與指導老師溝通并作出修改;另外,購買軸承、螺釘等緊固件與連接件,確定需要機械零件的參數與一般選定條件;電路與控制方面則開始進行相關子控制電路的制作,聲音控制系統(tǒng)與觸感控制系統(tǒng)的設計以及一般算法的編寫與設計。
機械機構部分 | 電路與控制部分 |
機械設計修改 購買緊固件、軸承等 確定零件參數規(guī)格 | 子控制電路設計與制作 一般算法設計與編寫 |
第四周工作比較繁重,主要集中在機械設計方面繼續(xù)定稿與制作,并提交到加工方進行受理與制作,而同時根據之前測得的機械臂的主體結構受力數據等方面進行電機參數的選擇,購買舵機與步進電機;電路與控制方面開始對新購買的舵機、步進電機、步進電機驅動器進行相應的調試,由于供貨商方面提供的資料并不完整,因此在電機的調試上出現很多的問題,如占空比的掌握、步進電機脈沖的頻率調整等未能使電機轉動。
機械機構部分 | 電路與控制部分 |
機械設計定稿 交付加工 確定電機參數并購買電機 | 對電機進行運動測試 |
第五周由于等待機械加工的原因,在機械方面并沒有太多的工作,只是一些購買簡單組裝工具的工作;而工作重心則主要都集中在整個團隊對于新的電機的調試方面,分別向《機器人設計與制作》任課老師、電工電子實驗中心老師、機電學院研究生等多方面進行相應的請教,終于使舵機能夠轉動,而步進電機則出現發(fā)出尖叫聲的問題。
機械機構部分 | 電路與控制部分 |
購買組裝工具 | 對電機進行運動測試 |
第六周機械加工完成,機械機構方面進行相應的組裝工作,主要集中在對機械臂主體的組裝、電機的裝入與固定、底座的制作等配套工作;而控制方面則對已經調整好的舵機進行相應的控制測試,同時再對步進電機進行調試,終于在使用高頻信號發(fā)生器之下使步進電機轉動,并且基本掌握步進電機的工作頻率與工作電壓,以及步進電機驅動的控制技巧。
機械機構部分 | 電路與控制部分 |
機械零件組裝 配套系統(tǒng)制作 | 舵機進行控制測試 步進電機運動測試 |
第七周需要加快工作進度,機械方面已經成型,結構已經得到確定,并且各個自由度運動在工作條件下運動正常;而工作都集中在電路與控制方面,主要是整體程序的編寫與各個子電路的測試,以及各個示教系統(tǒng)對電機的控制調試。
機械機構部分 | 電路與控制部分 |
自由度微調 | 整體與子程序編寫 各個子系統(tǒng)對電機調試 |
第八周主要工作在于整機調試以及編寫項目報告;整機調試工作主要集中在對機械臂的主體結構進行PIC單片機控制,模擬現場情況進行各個子控制系統(tǒng)的調試,模擬各種可能出現的問題進行相關的準備與設計。
整體工作 |
整體機械臂調試 編寫總結報告 模擬及設想問題及準備解決方案 |
3. 探究過程出現的問題與采取的解決方法
3.1
【問題】機械設計不規(guī)范,如軸、各級機械臂、連接件等方面都出現了如孔徑不均、鈑金件尺寸不規(guī)范、軸沒有分級等多種問題。
【解決方法】通過與指導教師的溝通與多次交流,并將機械設計圖交給老師審閱與修改,由指導教師提供改進的意見,我們通過4次的大型修改,最終確定了現在的機械結構:使用角鐵連接機械臂、多級軸結構、法蘭與軸承分邊固定等機械結構,并在11月中下旬終于將機械設計圖最終定稿并交付加工方進行相應的機械加工與制造。
3.2
【問題】整個機械臂主體加工精度過低,導致機械臂多個地方的連接出現問題,軸與軸承的配合過盈太多,主體結構的通孔間隙過大,平行機械臂結構的通孔沒有對準等多種機械加工精度問題。
【解決方法】由于本機器人是交付到外面的加工方進行相應的機械加工和機械制造,而為了節(jié)省成本,選擇了精度比較低的手控機床來進行加工,因此機械加工的精度存在相當的問題。為了解決上述的問題,我們在設計的時候已經相應增加了一定的過盈量以防加工失誤導致間隙過大等無法彌補的問題;同時,我們在獲得零件的時候已經進行相關的檢查,保證了一定的精度,但是仍然存在過盈或者間隙過大等一系列的問題,最終解決方法是采用我們自己進行后期再加工,如對軸、機械臂結構等進行打磨,同時對過盈量過大的工件進行銼刀慢銼以減低過盈量等,最終順利將所以工件組裝起來并完成機械臂的整體結構。
3.3
【問題】由于沒有完整的技術資料,舵機在給予一定脈沖的時候不轉動,或者出現跳動等現象。
【解決方法】出于沒有相關技術資料的原因,我們首先是在網上尋找MG945這款舵機的相關控制資料,但是沒有完整的占空比的信息,因此我們又找到了電工電子實驗中心的同學、《機器人設計與制作》的任課老師咨詢,最終綜合網上的論壇資料與老師提供的信息,最后利用單片機進行脈寬調節(jié),最終確定了低電平延時20ms,高電平在1.5±1ms的伺服信號,并且增加循環(huán)等程序的控制下使電機能夠按照一定的周期擺動,并且在裝上機械臂后能夠使機械臂按照一定的角度進行上下的擺動。
3.4
【問題】由于沒有完整的技術資料,步進電機在使用單片機控制下不轉動、或者跳動并發(fā)出尖銳聲音
【解決方法】同樣由于技術資料不完整以及供貨方方面提供的信息有誤,步進電機不能正常轉動,其主要現象是前者在步進電機驅動上電的情況之下步進電機對信號完全沒有反應;后則則是在給予的脈沖有反映,但發(fā)出聲音。我們在面對著兩種情況的時候分別尋找了機電學院的研究生與電工電子技術的老師進行相關的咨詢和調試,最后根據相關的資料,得出了步進電機的控制方法是通過一個高低電平的變化使步進電機發(fā)生一個步矩角的轉動的相關信息,可以確定是由于我們在連接的時候,給予步進電機的脈沖頻率過高,達到了10000Hz,遠超過步進電機的反應速度,因此后來將頻率調至350-700Hz左右的時候,步進電機就能夠正常工作。
3.5
【問題】步進電機運動與其步矩角參數等理論值不對應,如給定50個高低變化的脈沖,但是步進電機轉動的角度大于90度。
【解決方法】我們最終選擇忽略這個問題,盡管無法量化地控制電機,但是通過多次的調試,最終能夠將電機的轉角調到一個合適的范圍;另外,根據供貨方提供的信息,本問題的成因很可能是由于步進電機的驅動器有個細分數可選,而這個細分數可以將步矩角細分到一定的范圍,因此可能導致給定的高低電平變化使步進電機的輸出角度變小,從而是角度與其原來的理論值不相符。
3.6
【問題】步進電機與其電機驅動需要較高電壓電源,而且電機需要一個獨立電源,單片機需要一個獨立邏輯電源。
【解決方法】按照原來的計劃,我們是打算利用7805芯片進行相應的電壓調整,使整個機器人只需要一個電源系統(tǒng),但是后來根據指導老師的建議,認為邏輯電源與外接電源不宜一致,因此,應該使用兩個獨立電源;同時,由于步進電機的轉動最低要求9V的電壓,而最好是有15V的電壓,因此在參加比賽的時候,則需要使用到外加的學生電源進行相關的驅動。
3.7
【問題】兩個舵機使用程序無法進行調平
【解決方法】出現這個問題的根本原因是由于選用的舵機有個機械零點,它具有上電自動復位的功能,因此在使用的時候就會由于每次單片機上電是舵機復位而導致無法進行調平,無論我們的程序怎樣編寫都沒有辦法使其合適擊鼓的需要。為了解決這個問題,我們只能采用機械調平的方法處理,通過調整機械臂與舵機的不同角度的連接,從而改變其初始的角度,以及將兩個機械臂調成平行的,使其合適擊鼓的狀態(tài)。
3.8
【問題】擊鼓的時候擊打的聲音并不明顯
【解決方法】這個問題的主要成因是由于舵機屬于高扭矩的舵機,而其代價則是其轉速比較慢,僅為0.25sec/60°,同時在電機的伺服控制當中為了保證其有足夠的轉角,其速度也不能調得過快,否則會導致第三級機械臂與舵機之間的連接出現較大的問題。為了解決這個問題,我們只能夠在轉速、轉角、擊打力度之間取得一個平衡,我們也多次改變舵機的伺服控制程序,包括對高低電平的脈寬進行相關調節(jié),以及對循環(huán)的次數進行調節(jié),只能得出現在的這個情況,即便是在以后進行改進的時候,也只能在硬件方面,如增加鼓棒重量,改變鼓的結構或者使用架子鼓的發(fā)聲比較強的鼓進行展示。
3.9
【問題】聲音傳感器過靈敏,以致超過了舵機的反應速度
【解決方法】這個問題的主要成因是由于聲音傳感器本身的精度問題引起的,因此解決的方法有多種,一個是調節(jié)聲音傳感器本身,而另外一種則是在聲音傳感器與單片機的相連的電路系統(tǒng)中增加可變電阻系統(tǒng),我們選擇了后者;在電路中增加可調的可變電阻,然后通過現場對可變電阻的調節(jié),能夠滿足對聲音傳感器在其工作范圍內的聲音探測能力的調制,進而能夠是機器人擁有更為廣泛的適應性與適用性。
3.10
【問題】部分電路不穩(wěn)定,使在聲控模式下舵機運動并不規(guī)律,如在一次聲音信號輸入之下,舵機重復運動。
【解決方法】這個問題的主要成因比較復雜,可能是由于電路的原因造成的,也可能是由于程序的編寫失誤或者是調用失誤導致的,因此至今我們也沒能明確這個問題的主要成因是什么。但是我們采用了多種手段,包括調整程序的結構、重新編寫子程序與調用的結構、重新設計電路與重新布局排版、增加施密特觸發(fā)器進行整波等手段,最終使聲控電路能夠正常工作。
六、評價與改進方案
1. 自我總結與評價
【自我總結】
經過2個月的設計與制作,終于將機器人完整地完成 ,可以說著2個月過得痛并快樂著。畢竟這個機器人是整個項目組成員已經經過了近半年的理論論證最終得到的實體,雖然其功能與原來的設想并不完全一致,但是至少給了我們一個參加比賽的機會,同時給了一個制作一臺真正機器人實體的機會,因此我們要感謝這次大賽,也感謝每一個支持與幫助過我們的老師、同學。
機器人的設計與制作總是伴隨著那幾個不可缺少的基本點,之所以我們參加比賽一來是為了更加貼近現實的機器人學,二來是為了對項目的后續(xù)發(fā)展帶來更大的幫助。因此我們選擇了制造一直機械臂,雖然參加比賽的功能目前還只是擊鼓,但是相信在更長遠的未來,它會走得更遠。
正如之前所說的,我們已經經過將近半年的理論驗證,從很多地方已經省下了不少時間,包括我們已經收集了足夠多的資料,也已經對機械臂的實體進行過相關的簡易建模研究,因此在參加本次比賽的時候能夠得心應手。而在這次比賽當中,我們的確學到了更多,雖然在之前理論驗證階段已經有了一定的數據與相關資料,但是仍然缺乏實際操作的空間。而在這次制作大白菜的過程中,首先學到的是機械設計方面的知識,在設計機械臂的時候無可避免地要與機械設計打交道,盡管之前有過相關的經驗,但是仍然不足以應對十幾樣零件的設計,因此我們在很多地方也做得不足,也在很多細節(jié)上存在很多漏洞;然后學到的就是加工方面的一些知識,雖然我們不是親自去加工這些零件,但是怎樣才能更加快更加省錢的完成這些零件的制造,是我們需要考慮的問題,因此我們省略了很多不必要的設計,也減去了一些在本次比賽不需要的自由度和相關零件;還有學到了電機的控制方面的知識,包括舵機、步進電機、直流電機的控制方法,是如何進行PWM脈沖調節(jié),或者是如果控制步進電機的這些知識有了初步的了解;最后就是電路與程序方面的調試,盡管電路并不復雜,但是整體由于機構比較大、功能復雜,因此各個組合的配合需要花很多心思去完成。
總結這次參加比賽,我們得到了一個能夠擊鼓的機器人,收獲了包括機械、電子、控制方面的知識,也認識到自己很多地方的不足與改進之處。
【自我評價】
盡管每個人的參與程度與參與范圍不同,但是整個項目組都處于一種合一的狀態(tài)。同時,每個人根據自己的能力所在進行相應方面的研究,由于我們設計與制作的不是一架普通的小車,而是一個機械臂,我們也需要在受力分析、三維建模、CAD制圖、機械加工與制造方面投入更多的時間,因此項目組成員主要都是偏機械方向,而在電路設計、程序控制、電機調測等方面也有相應的投入。由于工作組的不懈努力,無論是前期的準備,中期的制作與裝配,還是后期的調試都進行得有條不紊,成員各司所職,將自己本分做好,也去幫助遇到困難的同伴,大家將自己所知道的,所掌握的知識投入到本次比賽中,再從中收獲更多的知識。
2. 自我改進方案
【改進方案】
由于參加本次比賽是已經經過相當多理論的驗證,因此在比賽之后,我們可以根據原來設定的方案繼續(xù)進行機器人方面的探究。
根據原來的計劃,機械臂部分擁有大概9個自由度,其中第三級機械臂處會增加4個自由度,同時會為所有自由度配上舵機或者步進電機,同時也會在軸部分增加編碼盤或者其它能夠測量電機轉動角速度的器件,而在控制方面則著重與對機械臂轉速與轉動角度的量化測量,并且紀錄機械臂的對應時刻的位姿與速度,能夠完成測量臂的功能,并且能夠同時利用控制系統(tǒng)將其原來的示教過程中機械臂的位姿與速度重現,達到一種能夠記憶機械臂示教運動并重現的最終功能。
擁有這種功能的機器人的應用方面廣泛,而針對特定的情況以及我們的制作水平與機器人的規(guī)模大小,最終預計會確定用于醫(yī)療方面,例如針對中風病人進行按摩,在醫(yī)護人員進行相關的示教之后,機器人能夠自動重現示教的動作,對病人自動進行按摩。
附件:
1. 機械設計零件圖及裝配圖
1.1 機械零件CAD圖
【軸1】 1件
【軸2】 1件
【軸3】 1件
【底盤】 1件
【支架】 2件
【第一級機械臂】 2件
【第二級機械臂】 2件
【第三級機械臂】 2件
【二級臂連接件】 1件
【二三級連接件】 1件
【舵機固定件】 2件
【連接件1】 2件
【連接件2】 2件
1.2 機械零件UG圖
【軸1】 1件
【軸2】 1件
【軸3】 1件
【底盤】 1件
【支架】 2件
【第一級機械臂】 2件
【第二級機械臂】 2件
【第三級機械臂】 2件
【二級臂連接件】 1件
【二三級連接件】 1件
【舵機固定件】 2件
【連接件1】 2件
【連接件2】 2件
【軸承】5件
1.3 機械設計裝配總圖
【裝配圖-主視】
【裝配圖-俯視】
【裝配圖-任意角度1】
【裝配圖-任意角度2】
【裝配圖-任意角度3】
2. 電路原理圖
【聲控電路原理圖】
【觸控電路原理圖】
3. 全程序
#define uchar unsigned char
#define uint unsigned int
sbit P0_0=P0^0;
sbit P1_0=P1^0;
sbit P1_1=P1^1;
sbit P1_2=P1^2;
sbit P1_3=P1^3;
sbit P1_4=P1^4;
sbit P1_5=P1^5;
sbit P1_6=P1^6;
sbit P1_7=P1^7;
sbit P2_0=P2^0;
sbit P2_1=P2^1;
sbit P2_2=P2^2;
sbit P2_3=P2^3;
sbit P2_4=P2^4;
sbit P3_0=P3^0;
sbit P3_1=P3^1;
sbit P3_2=P3^2;
sbit P3_3=P3^3;
sbit P3_4=P3^4;
sbit P3_5=P3^5;
sbit P3_6=P3^6;
sbit P3_7=P3^7;
/***********************************/
void delay_nus(unsigned int i) //延時:i>=12 ,i的最小延時單12 us
{
i=i/10;
while(--i);
}
void delay_nms(unsigned int n) //延時n ms
{
n=n+1;
while(--n)
delay_nus(900); //延時 1ms,同時進行補償
}
/**********************************/
void init(void) //聲控程序,中斷初始化
{
EA=1;
EX0=1;
IT0=0;
}
void int0 () interrupt 0
{
P2_0=0;
delay_nms(200);
P2_0=1;
}
/***********************************/
void main(void) //主程序
{
int a,b,c;
P3_0=0;
while(1)
{
a=P3_4; //KEY4
if(a==0)
{
delay_nms(10);
if(a==0)
{
P2_3=0; //輸給自由表演的部分來檢測
while(1)
{
P1_5=0;
delay_nms(2000);
P2_3=1;
}
}
}
a=P3_5; //KEY5
if(a==0)
{
delay_nms(10);
if(a==0)
{
while(1)
{
b=P1_0;
c=P1_1;
if((b==0)(c==0))
{
P2_0=0;
delay_nms(200);
}
if((b==0)(c!=0))
{
P2_1=0;
delay_nms(200);
}
if((b!=0)(c==0))
{
P2_2=0;
delay_nms(200);
}
P1_6=0;
}
}
}
a=P3_6; //KEY6
if(a==0)
{
delay_nms(10);
if(a==0)
{
while(1)
{
init();
P1_7=0;
}}}
}}
#define uchar unsigned char
#define uint unsigned int
sbit P1_0=P1^0;
sbit P1_1=P1^1;
sbit P1_2=P1^2;
sbit P1_3=P1^3;
sbit P1_4=P1^4;
sbit P0_0=P0^0;
sbit P0_1=P0^1;
sbit P0_2=P0^2;
sbit P0_3=P0^3;
sbit P0_4=P0^4;
sbit P0_5=P0^5;
sbit P0_6=P0^6;
sbit P0_7=P0^7;
sbit P2_0=P2^0;
sbit P2_2=P2^2;
sbit P2_4=P2^4;
sbit P2_6=P2^6;
/***************************************/
void delay_nus(unsigned int i) //延時:i>=12 ,i最小延時12us
{
i=i/10;
while(--i);
}
void delay_nms(unsigned int n) //延時n ms
{
n=n+1;
while(--n)
delay_nus(900); //延時 1ms,同時進行補償
}
void SMotorL(void) //步進電機左轉
{
int i;
P1_4=1;
P1_3=1;
for(i=0;i1550;i++)
{
P1_2=1;
delay_nus(800);
P1_2=0;
delay_nus(800);
}
delay_nus(5000);
}
void SMotorR(void) //步進電機右轉
{
int i;
P1_4=1;
P1_3=0;
for(i=0;i1550;i++)
{
P1_2=1;
delay_nus(800);
P1_2=0;
delay_nus(800);
}
delay_nus(5000);
}
void DMotor_Dou(int n) //舵機雙手一起動,n表示次數
{
int i,j;
for(j=0;jn;j++)
{
for(i=0;i18;i++)
{
P1_0=1;
P1_1=1;
delay_nus(2400);
P1_0=0;
P1_1=0;
delay_nms(20);
}
for(i=0;i18;i++)
{
P1_0=1;
P1_1=1;
delay_nus(1000);
P1_0=0;
P1_1=0;
delay_nms(20);
}
}
delay_nms(10);
}
void DMotor_Dou1(int n) //舵機雙手一起動,n表示次數
{
int i,j;
for(j=0;jn;j++)
{
for(i=0;i30;i++)
{
P1_0=1;
P1_1=1;
delay_nus(2400);
P1_0=0;
P1_1=0;
delay_nms(20);
}
for(i=0;i30;i++)
{
P1_0=1;
P1_1=1;
delay_nus(600);
P1_0=0;
P1_1=0;
delay_nms(20);
}
}
delay_nms(10);
}
void DMotor_Dou2(int n) //舵機雙手一起動,n表示次數
{
int i,j;
for(j=0;jn;j++)
{
for(i=0;i25;i++)
{
P1_0=1;
P1_1=1;
delay_nus(2400);
P1_0=0;
P1_1=0;
delay_nms(20);
}
for(i=0;i25;i++)
{
P1_0=1;
P1_1=1;
delay_nus(800);
P1_0=0;
P1_1=0;
delay_nms(20);
}
}
delay_nms(10);
}
void DMotor_Sig(int n) //左右手交替,n代表次數
{
int i,j;
for(j=0;jn;j++)
{
for(i=0;i15;i++)
{
P1_1=1;
delay_nus(2400);
P1_1=0;
delay_nms(20);
}
for(i=0;i15;i++)
{
P1_1=1;
delay_nus(1000);
P1_1=0;
delay_nms(20);
}
for(i=0;i15;i++)
{
P1_0=1;
delay_nus(2400);
P1_0=0;
delay_nms(20);
}
for(i=0;i15;i++)
{
P1_0=1;
delay_nus(1000);
P1_0=0;
delay_nms(20);
}
}
delay_nms(10);
}
void DMotorL(int n) //舵機左手,n代表次數
{
int i,j;
for(j=0;jn;j++)
{
for(i=0;i18;i++)
{
P1_0=1;
delay_nus(2400);
P1_0=0;
delay_nms(20);
}
for(i=0;i18;i++)
{
P1_0=1;
delay_nus(900);
P1_0=0;
delay_nms(20);
}
}
delay_nms(10);
}
void DMotorR(int n) //舵機右手,n代表次數
{
int i,j;
for(j=0;jn;j++)
{
for(i=0;i18;i++)
{
P1_1=1;
delay_nus(2400);
P1_1=0;
delay_nms(20);
}
for(i=0;i18;i++)
{
P1_1=1;
delay_nus(900);
P1_1=0;
delay_nms(20);
}
}
delay_nms(10);
}
void main(void)
{
int a,b;
while(1)
{
a=P1_0;
if(a==0)
{
DMotor_Dou(1);
delay_nms(10);}
a=P1_1;
if(a==0)
{
DMotorL(1);
delay_nms(10);
}
a=P1_2;
if(a==0)
{
DMotorR(1);
delay_nms(10);}
a=P1_3;
if(a==0)
{
DMotor_Dou1(1);
DMotor_Dou2(1);
DMotor_Dou(3);
delay_nms(200);
DMotor_Dou(2);
SMotorR();
DMotor_Dou(2);
SMotorL();
DMotor_Dou(2);
SMotorR();
DMotor_Dou(2);
SMotorL();
DMotor_Dou(1);
delay_nms(30);
DMotor_Sig(2);
SMotorR();
DMotor_Sig(2);
SMotorL();
DMotorL(1);
DMotorR(1);
DMotorL(1);
DMotorR(1);
DMotor_Sig(1);
SMotorR();
DMotor_Sig(1);
DMotorL(1);
DMotorR(1);
DMotor_Dou(2);
delay_nms(100);
DMotorR(2);
DMotorL(2);
SMotorL();
DMotor_Sig(1);
DMotor_Sig(1);
delay_nms(100);
DMotorL(2);
SMotorR();
DMotorR(1);
DMotorL(2);
SMotorL();
DMotorR(1);
DMotorL(2);
SMotorR();
DMotorR(1);
DMotorL(2);
SMotorL();
DMotorR(1);
DMotor_Sig(1);
SMotorR();
DMotor_Sig(1);
DMotorL(2);
DMotor_Sig(1);
DMotorL(2);
DMotorR(2);
delay_nms(400);
DMotor_Sig(1);
SMotorL();
DMotorL(1);
DMotorR(1);
SMotorR();
DMotorL(1);
DMotorR(1);
SMotorL();
DMotorL(1);
DMotorR(1);
SMotorR();
DMotorL(1);
DMotorR(1);
DMotor_Sig(1);
DMotor_Sig(1);
SMotorL();
DMotorL(1);
DMotorR(1);
DMotor_Sig(1);
SMotorR();
DMotorL(1);
DMotorR(1);
DMotor_Sig(1);
SMotorL();
DMotorL(1);
DMotorR(1);
DMotor_Sig(1);
SMotorR();
DMotorL(1);
DMotorR(1);
DMotor_Sig(1);
delay_nms(100);
DMotorL(1); SMotorL();
DMotorR(1); SMotorR();
DMotorL(1); SMotorL();
DMotorR(1); SMotorR();
delay_nms(200);
DMotor_Dou(2);
DMotor_Dou1(1);
DMotor_Dou2(1);}
}
}
評論