【正文】 1 測(cè)試結(jié)果及分析 .................................................................................................11 測(cè)試結(jié)果 (數(shù)據(jù) ) .........................................................................................11 測(cè)試分析與結(jié)論 ....................................................................................... 12 5 總結(jié) ....................................................................... 12 附件 1：系統(tǒng)電路原理圖 ....................................................... 14 附件 2：電機(jī)控制源程序 ....................................................... 15 1 簡(jiǎn)易旋轉(zhuǎn)倒立擺及控制裝置 （ C 題） 【 本科 組】 1 系統(tǒng)方案 本系統(tǒng)主要由 主控 模塊、 角度傳感器 模塊、 機(jī)械結(jié)構(gòu) 模塊、電源模塊 等幾部分 組成，下面分別論證這幾個(gè)模塊的選擇。 選擇減速 直流電機(jī)帶動(dòng)旋轉(zhuǎn)臂旋轉(zhuǎn)， 采用增量式旋轉(zhuǎn)編碼器測(cè)量擺桿的角度 ，單片機(jī)輸 出 占空比可變的 PWM 波控制電機(jī) 角加速度 ，從而控制電機(jī)的加速度和給擺桿固定軸心的扭矩，實(shí)現(xiàn)控制擺桿的擺動(dòng)幅度并達(dá)到 和保持平衡狀態(tài)。 增量式編碼器是將位移轉(zhuǎn)換成周期性的電信號(hào)，再把這個(gè)電信號(hào)轉(zhuǎn)變成計(jì)數(shù)脈沖，用脈沖的個(gè)數(shù)表示位移的大小。 360 度旋轉(zhuǎn) 舵機(jī) 內(nèi)置驅(qū)動(dòng)電路，用單片機(jī)輸出的信號(hào)可以直接驅(qū)動(dòng)，接線方便，但是舵機(jī)的轉(zhuǎn)速普遍很低，加速度不能達(dá)到理想的要求。 綜合考慮采用 方案二 。而對(duì)于電機(jī)的控制，是通過(guò)檢測(cè)到反饋到單片機(jī)的擺桿擺動(dòng)的角度，然后單片機(jī)輸出 PWM 控制一個(gè)小的加速度給電機(jī)，這個(gè)加速度擺桿向反方向擺動(dòng)更大的角度。然后每經(jīng)過(guò)一個(gè) Z 相信號(hào)，都會(huì)將角度標(biāo)準(zhǔn)重新標(biāo)定一下（標(biāo)定 0 度），之后我們發(fā)現(xiàn)這個(gè)標(biāo)定很有必要，因?yàn)閿[桿在最低點(diǎn)的往復(fù)擺中，由于方向時(shí)刻改變，而測(cè)速脈沖（編碼器 A 相信號(hào)和 B相信號(hào)異或之后的信號(hào)，相當(dāng)于二分頻）在方向不同是會(huì)有一個(gè)相移，導(dǎo)致測(cè)速脈沖有一個(gè)累積誤差。再結(jié)合我們手頭可以得到的關(guān)于直流電機(jī)的 各種參數(shù)，我們很難精確地算出時(shí)間常數(shù)，只能近似，而且再加上程序的控制周期遠(yuǎn)小于時(shí)6 間常數(shù)，綜合考慮之后， 我們選擇了忽略公 式 9 中等號(hào)右邊的后兩項(xiàng)變量，也就是近似認(rèn)為直流電機(jī)的控制電壓與加速度成正比關(guān)系，這樣便于單片機(jī)給出控制信號(hào)。 電機(jī)控制程序內(nèi)容： 見(jiàn)附件 3 程序流程圖 主程序 與子程序框圖 8 圖 3 主程序與子程序框圖 “ 起擺做圓周運(yùn)動(dòng)” 子程序流程圖 9 開 始檢 測(cè) 擺 桿 角 度查 表 計(jì) 算 電 機(jī) 加 速度 并 輸 出 控 制 信 號(hào)相 鄰 兩 次 檢 測(cè) 到 的 擺 桿 速 度是 否 反 號(hào) 或 一 次 為 0否查 表 計(jì) 算 電 機(jī) 加 速 度 并 輸 出方 向 相 反 的 控 制 信 號(hào)是結(jié) 束 圖 4 “ 起擺做圓周運(yùn)動(dòng)” 子程序流程圖 10 “ 起擺并盡快達(dá)到穩(wěn)態(tài)” 子程序流程圖 開始檢測(cè)擺桿角度計(jì)算加速度并發(fā)出控制信號(hào)是否接近180平衡位置計(jì)算加速度是測(cè)量旋轉(zhuǎn)臂的速度給出控制信號(hào)否 圖 5 “起擺并達(dá)到倒立平衡狀態(tài)”子程序流程圖 11 4 測(cè)試方案與測(cè)試結(jié)果 測(cè)試方案 硬件軟件聯(lián)調(diào) ： 調(diào)試時(shí)，下載線連接單片機(jī)，以便程序執(zhí)行后實(shí)時(shí)觀測(cè)控制算法中多個(gè)變量，如擺桿角度，方向，電機(jī)速度以及加速度等等。 測(cè)試次數(shù) 1 2 3 4 5 6 7 8 保持時(shí)間 （ s） 5 5 5 5 5 5 5 5 旋轉(zhuǎn)臂最大轉(zhuǎn)動(dòng)角 （度） 90 90 90 90 90 90 90 90 12 測(cè)試功能四：從擺桿處于自然下垂?fàn)顟B(tài)開始，控制旋轉(zhuǎn)臂做往復(fù)旋轉(zhuǎn)運(yùn)動(dòng)，盡快使擺桿擺起倒立，保持倒立時(shí)間不小于 10s 測(cè)試次數(shù) 1 2 3 4 5 6 7 8 擺起時(shí)間 （ s） 2 3 1 2． 5 1 4 保持時(shí)間（ s） 10 10 0 10 10 10 10 10 測(cè)試功能五：在擺桿保持倒立狀態(tài)下，施加干擾后擺桿能繼續(xù)保持倒立或 2s 內(nèi)恢復(fù)倒立狀態(tài)。在第四天的下午，離比賽結(jié)束還有幾個(gè)小時(shí)時(shí)，我們放棄了一直堅(jiān)持的程序主線 —— 將加速度環(huán)作為內(nèi)環(huán)，速度環(huán)作為外環(huán)進(jìn)行控制，轉(zhuǎn)而采用了將速度環(huán)作為內(nèi)環(huán)，而加速度環(huán)作為外環(huán)，消除了速度的采樣滯后，才取得了比較可喜的進(jìn)步，但是由于時(shí)間實(shí)在太有限，我們沒(méi)有機(jī)會(huì)將所有部分都完美的移植在新的程序思路上。191。181。196。202。202。 GND VR? VR IN1 5 IN2 7 IN3 10 IN4 12 ENA 6 ENB 11 VSS 9 VS 4 OUT1 2 OUT2 3 OUT3 13 14 OUT4 ISEN A 1 ISEN B 15 U3 L298N D1 D2 D3 D4 D5 D6 D7 D8 +12V GND GND GND + C3 100UF C4 C6 + C5 100UF +5V +5V 1 2 3 4 J1 CON4 1 IRd/ 2 1D 3 1CP 4 ISd 5 1Q 6 1Q/ 7 GND 14 VSS 13 2Rd/ 12 2D 11 2CP 10 2Sd 9 2Q 8 2Q/ * Component_1 P1 P5 1 NC 2 VCC 3 GND 4 NC 5 A/ 6 A 7 B/ 8 B 9 I/ 10 I 1 189 2 +5v 3 GND 4 192 5 193 6 P7 7 195 8 P6 9 197 10 P4 U5 2 1B 3 1Y 4 2A 5 2B 6 2Y 7 GND 1 1A 14 VCC 13 4B 12 4A 11 4Y 10 3B 9 3A 8 3Y U6 74LS86210。 //unsigned char ii。 PTJ=PTJ|0XFF。 } if(rightdir == 0) RightMotorSpeed = (int)nRightPulse。 } else { PWMDTY45 = nLeftVal + 120。amp。 //SPEED_CONTROL_P = 0。((GravityAngle 0)amp。amp。 } if ((g_ucLeft == 1)amp。 POSITION_CONTROL_D = 。 } fD = POSITION_CONTROL_D*AngleSpeed。 RightMotorSpeedErr0 = RightMotorSpeedErr1。 SPEED_CONTROL_I = 1。 SpeedControlOut += fP + fI + fD。 for(delay_c=0。 float SPEED_CONTROL_P,SPEED_CONTROL_I,SPEED_CONTROL_D。 //long g_lPositionGoal。 //float fI1 = 0。 XINT_init()。 23 if(PositionControlCount == POSITION_CONTROL_COUNT) { if(RightMotorSpeedGoal RightMotorSpeedGoal1) RightMotorSpeedGoal += 。 if((PORTB_PB1 == 0) || (PORTB_PB3 == 0) || (PORTB_PB4 == 0)) MotorOutput()。 SPEED_CONTROL_I = 。 } //pragma CODE_SEG DEFAULT //后續(xù)代碼置于默認(rèn)區(qū)域內(nèi) 24 。amp。 //PositionControl()。 GetMotorPulse()。 TIM_init()。 float RightMotorSpeedErr0,RightMotorSpeedErr1,RightMotorSpeedErr2。 //int g_iZOffset。 } 主函數(shù)： include include include include const float tancode[181]= { ,6, ,63, ,13, ,49, 21 ,03, ,15, ,0, ,713, ,2.4751, ,1.4826,