【正文】 elta 機(jī)器人機(jī)構(gòu)圖 計(jì)算空間機(jī)構(gòu)自由度 KutzbachGrubler 公式可知： 武漢科技大學(xué)本科畢業(yè)設(shè)計(jì) 18 式中： n 為機(jī)構(gòu)桿件數(shù)； g 為運(yùn)動(dòng)副個(gè)數(shù)； 為所有運(yùn)動(dòng)副自由度之和。運(yùn)動(dòng)學(xué)逆問(wèn)題的求解是機(jī)器人控制的關(guān)鍵，因?yàn)橹挥惺垢麝P(guān)節(jié)變量按反解中求得的值運(yùn)動(dòng)，才能使末端操作器達(dá)到所要求的位姿?？紤]到運(yùn)動(dòng)平臺(tái)只有平動(dòng)而無(wú)轉(zhuǎn)動(dòng)，相對(duì)靜平臺(tái)姿態(tài)固定，機(jī)構(gòu)中所有平行四邊形框架始終為平面四邊形，不會(huì)扭曲為空間四邊形。 因此，在運(yùn)動(dòng)分析時(shí)，可以將機(jī)構(gòu)精簡(jiǎn)為圖 所示。三根主動(dòng)桿為圖中的 ，長(zhǎng)度均為 Lb；從動(dòng)桿為圖中 ,長(zhǎng)度均為 La。由圖可以看出，在固定平臺(tái)上建立坐標(biāo)系 OX Y Z，動(dòng)平臺(tái)上建立坐標(biāo)系 ，為電機(jī)驅(qū)動(dòng)臂對(duì)基座平臺(tái)的張角。 也可以得到點(diǎn) 在坐標(biāo)系 中的位置矢量： 其中， 。上式整理簡(jiǎn)化，得 . (2) 。 因此，當(dāng)給定機(jī)器人運(yùn)動(dòng)平臺(tái)的位置根據(jù)下式直接可求出電機(jī)的輸入，即驅(qū)動(dòng)臂的張角。 本次設(shè)計(jì) Delta 機(jī)器人機(jī)構(gòu)各參數(shù)為： R = 42mm， r = 18mm， La = 243mm，Lb = 120mm，動(dòng)平臺(tái)的位置 (x,y,z) = (50,50,100)(mm),經(jīng)計(jì)算得： 武漢科技大學(xué)本科畢業(yè)設(shè)計(jì) 22 A1 = B1 = C1 = A2 = B2 = C2 = A3 = B3 = 148 C3 = 武漢科技大學(xué)本科畢業(yè)設(shè)計(jì) 23 3 控制系統(tǒng)實(shí)現(xiàn) 控制系統(tǒng)控制流程 圖 控制系統(tǒng)流程圖 工作總流程： 用戶輸入坐標(biāo)命令傳遞給解算函數(shù)，解算函數(shù) (x,y,z） (α ,β ,γ )，經(jīng)解算函數(shù)反解后將所需運(yùn)轉(zhuǎn)步數(shù)傳遞到串口，串口將命令更新保存在機(jī)器狀態(tài)中。 S3C6410 初始化 S3C6410 的啟動(dòng)代碼的作用為硬件的初始化及調(diào)用 C 函數(shù)。 用戶 PC 串口服務(wù)程序 機(jī)械運(yùn)動(dòng)控制程序（中斷內(nèi)） 坐 標(biāo) 反 解 解 算 函 數(shù)(x,y,z)=(α ,β ,γ ) 步進(jìn)電機(jī)驅(qū)動(dòng)程序（定時(shí)中斷內(nèi)） 步進(jìn)電機(jī)驅(qū)動(dòng) 步進(jìn)電機(jī)驅(qū)動(dòng) 步進(jìn)電機(jī)驅(qū)動(dòng) 步進(jìn)電機(jī) 1 步進(jìn)電機(jī) 2 步進(jìn)電機(jī) 3 機(jī)械結(jié)構(gòu) 反饋 S3C6410 判斷步進(jìn)電機(jī)運(yùn)動(dòng)方向與所需運(yùn)動(dòng)步數(shù) 武漢科技大學(xué)本科畢業(yè)設(shè)計(jì) 24 圖 S3C6410 初始化流程 看門狗是 S3C6410 內(nèi)的一個(gè)定時(shí)器，其作用是在 CPU 啟動(dòng)后， 3s 內(nèi)若無(wú)任何對(duì)看門狗寄存器的操作將會(huì)強(qiáng)制復(fù)位系統(tǒng)。 S3C6410 內(nèi)有 8K 的片內(nèi)內(nèi)存，上電后從 0 地址開始運(yùn)行，但是本次設(shè)計(jì)的代碼大于 8K，程序無(wú)法完整復(fù)制至片內(nèi)內(nèi)存，需要將程序從 NAND flash 中復(fù)制到 DDR 內(nèi)運(yùn)行。 圖 程序復(fù)制至 DDR DDR 內(nèi)存初始化 程序的重定位 程序復(fù)制至 DDR 內(nèi)存 清除 BSS 段 Start 跳轉(zhuǎn)至 reset 關(guān)閉看門狗 設(shè)置棧 8K 初始化時(shí)鐘、 DDR、串口 代碼重定位 將程序從 NAND Flash復(fù)制到 DDR 內(nèi)存 清除 BSS 段 中斷初始化 打開中斷，進(jìn)入user mode bl main() 武漢科技大學(xué)本科畢業(yè)設(shè)計(jì) 25 DDR 內(nèi)存的初始化在 8K片內(nèi)內(nèi)存進(jìn)行，還未進(jìn)行重定位，只能用位置無(wú)關(guān)命令進(jìn)行初始化。 程序重定位是把代碼段、數(shù)據(jù)段復(fù)制到鏈接地址去，實(shí)現(xiàn)如下： adr r0, _start /* 獲得 _start 指令當(dāng)前所在的地址 : 0*/ ldr r1, =_start /* _start 的鏈接地址 0x51000000 */ ldr r2, =bss_start /* bss 段的起始鏈接地址 */ sub r2, r2, r1 程序從 NAND flash 復(fù)制到 DDR 內(nèi)存用 copy2ddr 函數(shù)實(shí)現(xiàn)，在進(jìn)行復(fù)制之前需要調(diào)用 nand_init()函數(shù)對(duì) NAND flash 進(jìn)行初始化。具體方法為向 BSS 段寫 ’0’實(shí)現(xiàn)內(nèi)存清零。還要對(duì) CPSR 寄存器第 7 位和 M[4:0]進(jìn)行設(shè)置，如圖 所示。 mrs r0, cpsr bic r0,r0,0x9f /* 清 cpsr 的 I 位 ,M4~M0 */ orr r0,r0,0x10 msr cpsr,r0 /* 進(jìn)入 user mode */ 圖 CPSR 寄存器格式 武漢科技大學(xué)本科畢業(yè)設(shè)計(jì) 26 中斷控制的實(shí)現(xiàn) 圖 中斷控制流程圖 按鍵中斷實(shí)現(xiàn)驅(qū)動(dòng)信號(hào)頻率的調(diào)節(jié)，即調(diào)節(jié)步進(jìn)電機(jī)的運(yùn)轉(zhuǎn)速度。對(duì)頻率進(jìn)行一定數(shù)值的調(diào)節(jié)（增加或減少）。 按下按鍵或 UART 發(fā)送緩沖區(qū)少于 16byte 時(shí)發(fā)生中斷。 開始 中斷初始化： irp_init() 設(shè)置 GPN 引腳為外部中斷。 保存現(xiàn)場(chǎng) 處理中斷 異 bl do_irq 按鍵調(diào)節(jié) PWM 恢復(fù)現(xiàn)場(chǎng) 發(fā)生中斷 分 辨中斷 按鍵中斷，調(diào)用 ent0~3() UART中斷，調(diào)用 do_uart_irq() UART 發(fā)送數(shù)據(jù) 武漢科技大學(xué)本科畢業(yè)設(shè)計(jì) 27 保存現(xiàn)場(chǎng) 如果不做保存處理，那麼即使以后能按斷點(diǎn)地址返回到被中斷程序，但由于環(huán)境被破壞，原程序也無(wú)法正確運(yùn)行。 在通過(guò) VIC0INTENABLE、 VIC0INTSELECT屏蔽后，可以讀取 IRQ 中斷狀態(tài)寄存器 VIC0IRQSTATUS 的值便知中斷是否有效， VIC0IRQSTATUS 每一位對(duì)應(yīng) 0~31 號(hào)中斷源。 在 uart_irq_init()中 VIC1VECTADDR5 = uart_irq。 //處理函數(shù)地址賦值 VIC0VECTADDR1 = eint4_11_irq。 void do_irq(void) { int i = 0。 if (VIC0IRQSTATUS) { the_isr = VIC0ADDRESS。 EINT0PEND = 0x3f。 } else if (VIC1IRQSTATUS) { the_isr = VIC1ADDRESS。 武漢科技大學(xué)本科畢業(yè)設(shè)計(jì) 28 } } 恢復(fù)現(xiàn)場(chǎng) 返回發(fā)生中斷時(shí)的下一條指令地址。 圖 時(shí)鐘發(fā)生器結(jié)構(gòu)框圖 鎖相環(huán)配置 在配置鎖相環(huán)之前都會(huì)通過(guò)向時(shí)鐘源控制寄存器的 CLK_SRC[2:0]位寫 ’0’將系統(tǒng)時(shí)鐘選為 Fin 直通模式。本次設(shè)計(jì)沒(méi)有用到 EPLL，只需完成對(duì) APLL 于 MPLL 的配置。 APLL 和 MPLL 的輸出頻率計(jì)算公式： 本次設(shè)計(jì)所使用的頻率 。 對(duì) MPLL_CON 寄存器設(shè)置： MPLL_CON = ((131) | (266 16) | (3 8) | (1)) 分頻器的配置 為獲得不同的時(shí)鐘滿足不同功能模塊的需求，時(shí)鐘發(fā)生單元提供了若干個(gè)時(shí)鐘分頻器對(duì)鎖相環(huán)輸出的時(shí)鐘進(jìn)行分頻處理。本次設(shè)計(jì)中并沒(méi)有用到 LCD、 TV 定標(biāo)器等，只對(duì) CLK_DIV0 進(jìn)行設(shè)置。本次設(shè)計(jì)中， ARMCLK = 532MHz， HCLKX2 = 266MHz， HCLK = 133MHz，PCLK = 。 表 CLK_DIV0寄存器部分位 CLK_DIV0 位 描述 初始狀態(tài) PCLK_RATIO [15:12] PCLK = HCLKX2 / (PCLK_RATIO + 1) 0x1 HCLKX2_RATIO [11:9] HCLKX2 = HCLKX2IN / (HCLKX2_RATIO) 0x0 HCLK_RATIO [8] HCLK = HCLKX2 / (HCLK_RATIO + 1) 0 MPLL_RATIO [4] DOUTMPLL = MOUTMPLL / (MPLL_RATIO + 1) 0 ARM_RATIO [2:0] ARMCLK = DOUTAPLL / (ARM_RATIO + 1) 0x0 ARMCLK = DOUTAPLL / (ARM_RATIO + 1) ARMCLK = 532 / (0+1) = 532MHz HCLKX2 = HCLKX2IN / (HCLKX2_RATIO + 1) HCLKX2 = 532 / (1 + 1) = 266MHz HCLK = HCLKX2 / (HCLK_RATIO + 1) HCLK = 266 / (1 + 1) = 133MHz PCLK = HCLKX2 / (PCLK_RATIO + 1) PCLK = 133 / (3 + 1) = 需要 向 CLK_DIV0 寄存器設(shè)置可對(duì)以上分頻器進(jìn)行設(shè)置： CLK_DIV0 = (0) | (0 4) | (1 8) | (1 9) | (3 12) 串口控制 本次設(shè)計(jì)采用通用異步串行收發(fā)器簡(jiǎn)稱 UART 實(shí)現(xiàn)與上位機(jī)的通訊，S3C6410 在與上位機(jī)的通訊中，設(shè)置字符幀的格式為 1 位起始位， 8 位數(shù)據(jù)位，無(wú)校驗(yàn)位， 1 位停止位。本次設(shè)計(jì)并沒(méi)有用到流量控制，且自動(dòng)流量控制也不支持武漢科技大學(xué)本科畢業(yè)設(shè)計(jì) 30 RS232。通過(guò)配置 UART 通道控制寄存器 UCON0 的 [11:10]位選擇，系統(tǒng)上電后 UCON0寄存器的初始狀態(tài)就是選擇 PCLK，不用進(jìn)行設(shè)置。 配置波特率發(fā)生器 串口通訊的波特率發(fā)生器是由波特率分頻寄存器 UBRDIV0與分頻插槽寄存器 UDIVSLOT0 控制的。 本次設(shè)計(jì)中，波特率選擇 115200bps， DIV_VAL = 。 串口中斷控制 串口中斷，通過(guò)查詢 S3C6410 芯片手冊(cè)（如表 所示）可知， UART0 中斷號(hào)為 37 為中斷寄存器 VIC1，在串口中斷初始化函數(shù) uart_irq_init()中使能中斷，將中斷處理函數(shù) uart_irq()地址賦給 VIC1VECTADDR5。最后， UINTM0 = 0xF，關(guān)閉串口中斷。處理程序流程如圖 ： 圖 UART 串口中斷流程 發(fā)送數(shù)據(jù)時(shí)，先用 isFull()函數(shù)判斷發(fā)送 FIFO數(shù)據(jù)是否滿，若不滿用 putData()函數(shù)將數(shù)據(jù)放到發(fā)送 FIFO 中，開啟 UART 中斷 UINTM0 amp。 定時(shí)器設(shè)置 S3C6410 有 5 個(gè) 32 位定時(shí)器，這些定時(shí)器可以產(chǎn)生中斷到 ARM 子系統(tǒng)，定時(shí)器 T0、 T1 具有 PWM 功能，可以驅(qū)動(dòng)外部 I\O 口。因此，本設(shè)計(jì)采用的是內(nèi)部定時(shí)器 T2。當(dāng)向下計(jì)數(shù)到“ 0”時(shí)，定時(shí)器向 CPU 請(qǐng)求中斷，本輪計(jì)數(shù)完成，相應(yīng)的 TCNTB2 又會(huì)重新裝載，進(jìn)入到下一輪的向下發(fā)生中斷 判斷發(fā)送還是接收 從 RXFIFO 取出數(shù)據(jù) Buffer{}是否為空 Buffer[]取出數(shù)據(jù)放入 TXFIFO 禁止發(fā)送中斷 清除 UART 中斷 發(fā)送 接收 是 否 武漢科技大學(xué)本科畢業(yè)設(shè)計(jì) 32 計(jì)數(shù)。當(dāng)下計(jì)數(shù)器繼續(xù)向下計(jì)數(shù)到“ 0”時(shí)，輸出電平再次翻轉(zhuǎn)并產(chǎn)生中斷（如果中斷使能）。 圖 定時(shí)器邏輯框圖 本次設(shè)計(jì)定時(shí)器的時(shí)鐘源為 PCLK，為了實(shí)現(xiàn)對(duì)步進(jìn)電機(jī)的精確控制，對(duì)TCNTB2寄存器賦值 val = PCLK/1000000 – 1。 void usDelay(int duty) { unsigned int val = PCLK/1000000 1。= ~(0xff8)。 /* =66500000 / (0+1)/1 = 66500000 */ TCFG1 amp。 TCFG1 |= (08)。 /*頻率為 1000Hz*/ TCMPB2 = (val1)。= ~(0xf12)。 TCON amp。 /*清除手動(dòng)更新位 */ while(duty) { while(TCNTO2=(val1))。 } } 武漢科技大學(xué)本科畢業(yè)設(shè)計(jì) 33 步進(jìn)電機(jī)控制 電機(jī)控制采用精確延時(shí)后反轉(zhuǎn) I\O口電平產(chǎn)生 PWM 信號(hào)， GPC GPC GPC6為方向控制，高低電平控制正反轉(zhuǎn)。電機(jī)控制函數(shù)如下： void moto1(int pul1,int dir1,int duty1) /*電機(jī) 1控制 */ { unsigned int i。 GPCDAT |= (dir11)。ipul1。 /*GPC3 脈沖輸出 */ usDelay(duty1)。= ~(13)。 } } 上位機(jī)經(jīng)過(guò)坐標(biāo)反解得到運(yùn)動(dòng)步數(shù)，通過(guò)串口發(fā) 送給機(jī)械運(yùn)動(dòng)控制函數(shù)，函數(shù)判斷各步進(jìn)電機(jī)的方向與所需運(yùn)動(dòng)的步數(shù)。判斷的原理：與前一次所走步數(shù)進(jìn)行比較，若所需走的步數(shù)大于前一次的步數(shù)，則向下運(yùn)動(dòng)兩者之差，否則向上運(yùn)動(dòng)兩者之差，具體流程如圖 。 圖 上位機(jī)界面 上位機(jī)串口模塊 串口通信使用 VS2021 下的 MSComm 控件。 MSComm 控件在串口編程時(shí)非常方便，